Back to home page

OSCL-LXR
 
 

     

0001 // SPDX-License-Identifier: GPL-2.0-or-later
0002 /*
0003 
0004   Broadcom B43legacy wireless driver
0005 
0006   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
0007              Stefano Brivio <stefano.brivio@polimi.it>
0008              Michael Buesch <m@bues.ch>
0009              Danny van Dyk <kugelfang@gentoo.org>
0010              Andreas Jaggi <andreas.jaggi@waterwave.ch>
0011   Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
0012 
0013   Some parts of the code in this file are derived from the ipw2200
0014   driver  Copyright(c) 2003 - 2004 Intel Corporation.
0015 
0016 
0017 */
0018 
0019 #include <linux/delay.h>
0020 
0021 #include "b43legacy.h"
0022 #include "main.h"
0023 #include "phy.h"
0024 #include "radio.h"
0025 #include "ilt.h"
0026 
0027 
0028 /* Table for b43legacy_radio_calibrationvalue() */
0029 static const u16 rcc_table[16] = {
0030     0x0002, 0x0003, 0x0001, 0x000F,
0031     0x0006, 0x0007, 0x0005, 0x000F,
0032     0x000A, 0x000B, 0x0009, 0x000F,
0033     0x000E, 0x000F, 0x000D, 0x000F,
0034 };
0035 
0036 /* Reverse the bits of a 4bit value.
0037  * Example:  1101 is flipped 1011
0038  */
0039 static u16 flip_4bit(u16 value)
0040 {
0041     u16 flipped = 0x0000;
0042 
0043     B43legacy_BUG_ON(!((value & ~0x000F) == 0x0000));
0044 
0045     flipped |= (value & 0x0001) << 3;
0046     flipped |= (value & 0x0002) << 1;
0047     flipped |= (value & 0x0004) >> 1;
0048     flipped |= (value & 0x0008) >> 3;
0049 
0050     return flipped;
0051 }
0052 
0053 /* Get the freq, as it has to be written to the device. */
0054 static inline
0055 u16 channel2freq_bg(u8 channel)
0056 {
0057     /* Frequencies are given as frequencies_bg[index] + 2.4GHz
0058      * Starting with channel 1
0059      */
0060     static const u16 frequencies_bg[14] = {
0061         12, 17, 22, 27,
0062         32, 37, 42, 47,
0063         52, 57, 62, 67,
0064         72, 84,
0065     };
0066 
0067     if (unlikely(channel < 1 || channel > 14)) {
0068         printk(KERN_INFO "b43legacy: Channel %d is out of range\n",
0069                   channel);
0070         dump_stack();
0071         return 2412;
0072     }
0073 
0074     return frequencies_bg[channel - 1];
0075 }
0076 
0077 void b43legacy_radio_lock(struct b43legacy_wldev *dev)
0078 {
0079     u32 status;
0080 
0081     status = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
0082     B43legacy_WARN_ON(status & B43legacy_MACCTL_RADIOLOCK);
0083     status |= B43legacy_MACCTL_RADIOLOCK;
0084     b43legacy_write32(dev, B43legacy_MMIO_MACCTL, status);
0085     udelay(10);
0086 }
0087 
0088 void b43legacy_radio_unlock(struct b43legacy_wldev *dev)
0089 {
0090     u32 status;
0091 
0092     b43legacy_read16(dev, B43legacy_MMIO_PHY_VER); /* dummy read */
0093     status = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
0094     B43legacy_WARN_ON(!(status & B43legacy_MACCTL_RADIOLOCK));
0095     status &= ~B43legacy_MACCTL_RADIOLOCK;
0096     b43legacy_write32(dev, B43legacy_MMIO_MACCTL, status);
0097 }
0098 
0099 u16 b43legacy_radio_read16(struct b43legacy_wldev *dev, u16 offset)
0100 {
0101     struct b43legacy_phy *phy = &dev->phy;
0102 
0103     switch (phy->type) {
0104     case B43legacy_PHYTYPE_B:
0105         if (phy->radio_ver == 0x2053) {
0106             if (offset < 0x70)
0107                 offset += 0x80;
0108             else if (offset < 0x80)
0109                 offset += 0x70;
0110         } else if (phy->radio_ver == 0x2050)
0111             offset |= 0x80;
0112         else
0113             B43legacy_WARN_ON(1);
0114         break;
0115     case B43legacy_PHYTYPE_G:
0116         offset |= 0x80;
0117         break;
0118     default:
0119         B43legacy_BUG_ON(1);
0120     }
0121 
0122     b43legacy_write16(dev, B43legacy_MMIO_RADIO_CONTROL, offset);
0123     return b43legacy_read16(dev, B43legacy_MMIO_RADIO_DATA_LOW);
0124 }
0125 
0126 void b43legacy_radio_write16(struct b43legacy_wldev *dev, u16 offset, u16 val)
0127 {
0128     b43legacy_write16(dev, B43legacy_MMIO_RADIO_CONTROL, offset);
0129     b43legacy_write16(dev, B43legacy_MMIO_RADIO_DATA_LOW, val);
0130 }
0131 
0132 static void b43legacy_set_all_gains(struct b43legacy_wldev *dev,
0133                   s16 first, s16 second, s16 third)
0134 {
0135     struct b43legacy_phy *phy = &dev->phy;
0136     u16 i;
0137     u16 start = 0x08;
0138     u16 end = 0x18;
0139     u16 offset = 0x0400;
0140     u16 tmp;
0141 
0142     if (phy->rev <= 1) {
0143         offset = 0x5000;
0144         start = 0x10;
0145         end = 0x20;
0146     }
0147 
0148     for (i = 0; i < 4; i++)
0149         b43legacy_ilt_write(dev, offset + i, first);
0150 
0151     for (i = start; i < end; i++)
0152         b43legacy_ilt_write(dev, offset + i, second);
0153 
0154     if (third != -1) {
0155         tmp = ((u16)third << 14) | ((u16)third << 6);
0156         b43legacy_phy_write(dev, 0x04A0,
0157                     (b43legacy_phy_read(dev, 0x04A0) & 0xBFBF)
0158                     | tmp);
0159         b43legacy_phy_write(dev, 0x04A1,
0160                     (b43legacy_phy_read(dev, 0x04A1) & 0xBFBF)
0161                     | tmp);
0162         b43legacy_phy_write(dev, 0x04A2,
0163                     (b43legacy_phy_read(dev, 0x04A2) & 0xBFBF)
0164                     | tmp);
0165     }
0166     b43legacy_dummy_transmission(dev);
0167 }
0168 
0169 static void b43legacy_set_original_gains(struct b43legacy_wldev *dev)
0170 {
0171     struct b43legacy_phy *phy = &dev->phy;
0172     u16 i;
0173     u16 tmp;
0174     u16 offset = 0x0400;
0175     u16 start = 0x0008;
0176     u16 end = 0x0018;
0177 
0178     if (phy->rev <= 1) {
0179         offset = 0x5000;
0180         start = 0x0010;
0181         end = 0x0020;
0182     }
0183 
0184     for (i = 0; i < 4; i++) {
0185         tmp = (i & 0xFFFC);
0186         tmp |= (i & 0x0001) << 1;
0187         tmp |= (i & 0x0002) >> 1;
0188 
0189         b43legacy_ilt_write(dev, offset + i, tmp);
0190     }
0191 
0192     for (i = start; i < end; i++)
0193         b43legacy_ilt_write(dev, offset + i, i - start);
0194 
0195     b43legacy_phy_write(dev, 0x04A0,
0196                 (b43legacy_phy_read(dev, 0x04A0) & 0xBFBF)
0197                 | 0x4040);
0198     b43legacy_phy_write(dev, 0x04A1,
0199                 (b43legacy_phy_read(dev, 0x04A1) & 0xBFBF)
0200                 | 0x4040);
0201     b43legacy_phy_write(dev, 0x04A2,
0202                 (b43legacy_phy_read(dev, 0x04A2) & 0xBFBF)
0203                 | 0x4000);
0204     b43legacy_dummy_transmission(dev);
0205 }
0206 
0207 /* Synthetic PU workaround */
0208 static void b43legacy_synth_pu_workaround(struct b43legacy_wldev *dev,
0209                       u8 channel)
0210 {
0211     struct b43legacy_phy *phy = &dev->phy;
0212 
0213     might_sleep();
0214 
0215     if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6)
0216         /* We do not need the workaround. */
0217         return;
0218 
0219     if (channel <= 10)
0220         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
0221                   channel2freq_bg(channel + 4));
0222     else
0223         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
0224                   channel2freq_bg(channel));
0225     msleep(1);
0226     b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
0227               channel2freq_bg(channel));
0228 }
0229 
0230 u8 b43legacy_radio_aci_detect(struct b43legacy_wldev *dev, u8 channel)
0231 {
0232     struct b43legacy_phy *phy = &dev->phy;
0233     u8 ret = 0;
0234     u16 saved;
0235     u16 rssi;
0236     u16 temp;
0237     int i;
0238     int j = 0;
0239 
0240     saved = b43legacy_phy_read(dev, 0x0403);
0241     b43legacy_radio_selectchannel(dev, channel, 0);
0242     b43legacy_phy_write(dev, 0x0403, (saved & 0xFFF8) | 5);
0243     if (phy->aci_hw_rssi)
0244         rssi = b43legacy_phy_read(dev, 0x048A) & 0x3F;
0245     else
0246         rssi = saved & 0x3F;
0247     /* clamp temp to signed 5bit */
0248     if (rssi > 32)
0249         rssi -= 64;
0250     for (i = 0; i < 100; i++) {
0251         temp = (b43legacy_phy_read(dev, 0x047F) >> 8) & 0x3F;
0252         if (temp > 32)
0253             temp -= 64;
0254         if (temp < rssi)
0255             j++;
0256         if (j >= 20)
0257             ret = 1;
0258     }
0259     b43legacy_phy_write(dev, 0x0403, saved);
0260 
0261     return ret;
0262 }
0263 
0264 u8 b43legacy_radio_aci_scan(struct b43legacy_wldev *dev)
0265 {
0266     struct b43legacy_phy *phy = &dev->phy;
0267     u8 ret[13] = { 0 };
0268     unsigned int channel = phy->channel;
0269     unsigned int i;
0270     unsigned int j;
0271     unsigned int start;
0272     unsigned int end;
0273 
0274     if (!((phy->type == B43legacy_PHYTYPE_G) && (phy->rev > 0)))
0275         return 0;
0276 
0277     b43legacy_phy_lock(dev);
0278     b43legacy_radio_lock(dev);
0279     b43legacy_phy_write(dev, 0x0802,
0280                 b43legacy_phy_read(dev, 0x0802) & 0xFFFC);
0281     b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
0282                 b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
0283                 & 0x7FFF);
0284     b43legacy_set_all_gains(dev, 3, 8, 1);
0285 
0286     start = (channel > 5) ? channel - 5 : 1;
0287     end = (channel + 5 < 14) ? channel + 5 : 13;
0288 
0289     for (i = start; i <= end; i++) {
0290         if (abs(channel - i) > 2)
0291             ret[i-1] = b43legacy_radio_aci_detect(dev, i);
0292     }
0293     b43legacy_radio_selectchannel(dev, channel, 0);
0294     b43legacy_phy_write(dev, 0x0802,
0295                 (b43legacy_phy_read(dev, 0x0802) & 0xFFFC)
0296                 | 0x0003);
0297     b43legacy_phy_write(dev, 0x0403,
0298                 b43legacy_phy_read(dev, 0x0403) & 0xFFF8);
0299     b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
0300                 b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
0301                 | 0x8000);
0302     b43legacy_set_original_gains(dev);
0303     for (i = 0; i < 13; i++) {
0304         if (!ret[i])
0305             continue;
0306         end = (i + 5 < 13) ? i + 5 : 13;
0307         for (j = i; j < end; j++)
0308             ret[j] = 1;
0309     }
0310     b43legacy_radio_unlock(dev);
0311     b43legacy_phy_unlock(dev);
0312 
0313     return ret[channel - 1];
0314 }
0315 
0316 /* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
0317 void b43legacy_nrssi_hw_write(struct b43legacy_wldev *dev, u16 offset, s16 val)
0318 {
0319     b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_CTRL, offset);
0320     b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_DATA, (u16)val);
0321 }
0322 
0323 /* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
0324 s16 b43legacy_nrssi_hw_read(struct b43legacy_wldev *dev, u16 offset)
0325 {
0326     u16 val;
0327 
0328     b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_CTRL, offset);
0329     val = b43legacy_phy_read(dev, B43legacy_PHY_NRSSILT_DATA);
0330 
0331     return (s16)val;
0332 }
0333 
0334 /* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
0335 void b43legacy_nrssi_hw_update(struct b43legacy_wldev *dev, u16 val)
0336 {
0337     u16 i;
0338     s16 tmp;
0339 
0340     for (i = 0; i < 64; i++) {
0341         tmp = b43legacy_nrssi_hw_read(dev, i);
0342         tmp -= val;
0343         tmp = clamp_val(tmp, -32, 31);
0344         b43legacy_nrssi_hw_write(dev, i, tmp);
0345     }
0346 }
0347 
0348 /* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
0349 void b43legacy_nrssi_mem_update(struct b43legacy_wldev *dev)
0350 {
0351     struct b43legacy_phy *phy = &dev->phy;
0352     s16 i;
0353     s16 delta;
0354     s32 tmp;
0355 
0356     delta = 0x1F - phy->nrssi[0];
0357     for (i = 0; i < 64; i++) {
0358         tmp = (i - delta) * phy->nrssislope;
0359         tmp /= 0x10000;
0360         tmp += 0x3A;
0361         tmp = clamp_val(tmp, 0, 0x3F);
0362         phy->nrssi_lt[i] = tmp;
0363     }
0364 }
0365 
0366 static void b43legacy_calc_nrssi_offset(struct b43legacy_wldev *dev)
0367 {
0368     struct b43legacy_phy *phy = &dev->phy;
0369     u16 backup[20] = { 0 };
0370     s16 v47F;
0371     u16 i;
0372     u16 saved = 0xFFFF;
0373 
0374     backup[0] = b43legacy_phy_read(dev, 0x0001);
0375     backup[1] = b43legacy_phy_read(dev, 0x0811);
0376     backup[2] = b43legacy_phy_read(dev, 0x0812);
0377     backup[3] = b43legacy_phy_read(dev, 0x0814);
0378     backup[4] = b43legacy_phy_read(dev, 0x0815);
0379     backup[5] = b43legacy_phy_read(dev, 0x005A);
0380     backup[6] = b43legacy_phy_read(dev, 0x0059);
0381     backup[7] = b43legacy_phy_read(dev, 0x0058);
0382     backup[8] = b43legacy_phy_read(dev, 0x000A);
0383     backup[9] = b43legacy_phy_read(dev, 0x0003);
0384     backup[10] = b43legacy_radio_read16(dev, 0x007A);
0385     backup[11] = b43legacy_radio_read16(dev, 0x0043);
0386 
0387     b43legacy_phy_write(dev, 0x0429,
0388                 b43legacy_phy_read(dev, 0x0429) & 0x7FFF);
0389     b43legacy_phy_write(dev, 0x0001,
0390                 (b43legacy_phy_read(dev, 0x0001) & 0x3FFF)
0391                 | 0x4000);
0392     b43legacy_phy_write(dev, 0x0811,
0393                 b43legacy_phy_read(dev, 0x0811) | 0x000C);
0394     b43legacy_phy_write(dev, 0x0812,
0395                 (b43legacy_phy_read(dev, 0x0812) & 0xFFF3)
0396                 | 0x0004);
0397     b43legacy_phy_write(dev, 0x0802,
0398                 b43legacy_phy_read(dev, 0x0802) & ~(0x1 | 0x2));
0399     if (phy->rev >= 6) {
0400         backup[12] = b43legacy_phy_read(dev, 0x002E);
0401         backup[13] = b43legacy_phy_read(dev, 0x002F);
0402         backup[14] = b43legacy_phy_read(dev, 0x080F);
0403         backup[15] = b43legacy_phy_read(dev, 0x0810);
0404         backup[16] = b43legacy_phy_read(dev, 0x0801);
0405         backup[17] = b43legacy_phy_read(dev, 0x0060);
0406         backup[18] = b43legacy_phy_read(dev, 0x0014);
0407         backup[19] = b43legacy_phy_read(dev, 0x0478);
0408 
0409         b43legacy_phy_write(dev, 0x002E, 0);
0410         b43legacy_phy_write(dev, 0x002F, 0);
0411         b43legacy_phy_write(dev, 0x080F, 0);
0412         b43legacy_phy_write(dev, 0x0810, 0);
0413         b43legacy_phy_write(dev, 0x0478,
0414                     b43legacy_phy_read(dev, 0x0478) | 0x0100);
0415         b43legacy_phy_write(dev, 0x0801,
0416                     b43legacy_phy_read(dev, 0x0801) | 0x0040);
0417         b43legacy_phy_write(dev, 0x0060,
0418                     b43legacy_phy_read(dev, 0x0060) | 0x0040);
0419         b43legacy_phy_write(dev, 0x0014,
0420                     b43legacy_phy_read(dev, 0x0014) | 0x0200);
0421     }
0422     b43legacy_radio_write16(dev, 0x007A,
0423                 b43legacy_radio_read16(dev, 0x007A) | 0x0070);
0424     b43legacy_radio_write16(dev, 0x007A,
0425                 b43legacy_radio_read16(dev, 0x007A) | 0x0080);
0426     udelay(30);
0427 
0428     v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
0429     if (v47F >= 0x20)
0430         v47F -= 0x40;
0431     if (v47F == 31) {
0432         for (i = 7; i >= 4; i--) {
0433             b43legacy_radio_write16(dev, 0x007B, i);
0434             udelay(20);
0435             v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8)
0436                              & 0x003F);
0437             if (v47F >= 0x20)
0438                 v47F -= 0x40;
0439             if (v47F < 31 && saved == 0xFFFF)
0440                 saved = i;
0441         }
0442         if (saved == 0xFFFF)
0443             saved = 4;
0444     } else {
0445         b43legacy_radio_write16(dev, 0x007A,
0446                     b43legacy_radio_read16(dev, 0x007A)
0447                     & 0x007F);
0448         b43legacy_phy_write(dev, 0x0814,
0449                     b43legacy_phy_read(dev, 0x0814) | 0x0001);
0450         b43legacy_phy_write(dev, 0x0815,
0451                     b43legacy_phy_read(dev, 0x0815) & 0xFFFE);
0452         b43legacy_phy_write(dev, 0x0811,
0453                     b43legacy_phy_read(dev, 0x0811) | 0x000C);
0454         b43legacy_phy_write(dev, 0x0812,
0455                     b43legacy_phy_read(dev, 0x0812) | 0x000C);
0456         b43legacy_phy_write(dev, 0x0811,
0457                     b43legacy_phy_read(dev, 0x0811) | 0x0030);
0458         b43legacy_phy_write(dev, 0x0812,
0459                     b43legacy_phy_read(dev, 0x0812) | 0x0030);
0460         b43legacy_phy_write(dev, 0x005A, 0x0480);
0461         b43legacy_phy_write(dev, 0x0059, 0x0810);
0462         b43legacy_phy_write(dev, 0x0058, 0x000D);
0463         if (phy->analog == 0)
0464             b43legacy_phy_write(dev, 0x0003, 0x0122);
0465         else
0466             b43legacy_phy_write(dev, 0x000A,
0467                         b43legacy_phy_read(dev, 0x000A)
0468                         | 0x2000);
0469         b43legacy_phy_write(dev, 0x0814,
0470                     b43legacy_phy_read(dev, 0x0814) | 0x0004);
0471         b43legacy_phy_write(dev, 0x0815,
0472                     b43legacy_phy_read(dev, 0x0815) & 0xFFFB);
0473         b43legacy_phy_write(dev, 0x0003,
0474                     (b43legacy_phy_read(dev, 0x0003) & 0xFF9F)
0475                     | 0x0040);
0476         b43legacy_radio_write16(dev, 0x007A,
0477                     b43legacy_radio_read16(dev, 0x007A)
0478                     | 0x000F);
0479         b43legacy_set_all_gains(dev, 3, 0, 1);
0480         b43legacy_radio_write16(dev, 0x0043,
0481                     (b43legacy_radio_read16(dev, 0x0043)
0482                     & 0x00F0) | 0x000F);
0483         udelay(30);
0484         v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
0485         if (v47F >= 0x20)
0486             v47F -= 0x40;
0487         if (v47F == -32) {
0488             for (i = 0; i < 4; i++) {
0489                 b43legacy_radio_write16(dev, 0x007B, i);
0490                 udelay(20);
0491                 v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >>
0492                                  8) & 0x003F);
0493                 if (v47F >= 0x20)
0494                     v47F -= 0x40;
0495                 if (v47F > -31 && saved == 0xFFFF)
0496                     saved = i;
0497             }
0498             if (saved == 0xFFFF)
0499                 saved = 3;
0500         } else
0501             saved = 0;
0502     }
0503     b43legacy_radio_write16(dev, 0x007B, saved);
0504 
0505     if (phy->rev >= 6) {
0506         b43legacy_phy_write(dev, 0x002E, backup[12]);
0507         b43legacy_phy_write(dev, 0x002F, backup[13]);
0508         b43legacy_phy_write(dev, 0x080F, backup[14]);
0509         b43legacy_phy_write(dev, 0x0810, backup[15]);
0510     }
0511     b43legacy_phy_write(dev, 0x0814, backup[3]);
0512     b43legacy_phy_write(dev, 0x0815, backup[4]);
0513     b43legacy_phy_write(dev, 0x005A, backup[5]);
0514     b43legacy_phy_write(dev, 0x0059, backup[6]);
0515     b43legacy_phy_write(dev, 0x0058, backup[7]);
0516     b43legacy_phy_write(dev, 0x000A, backup[8]);
0517     b43legacy_phy_write(dev, 0x0003, backup[9]);
0518     b43legacy_radio_write16(dev, 0x0043, backup[11]);
0519     b43legacy_radio_write16(dev, 0x007A, backup[10]);
0520     b43legacy_phy_write(dev, 0x0802,
0521                 b43legacy_phy_read(dev, 0x0802) | 0x1 | 0x2);
0522     b43legacy_phy_write(dev, 0x0429,
0523                 b43legacy_phy_read(dev, 0x0429) | 0x8000);
0524     b43legacy_set_original_gains(dev);
0525     if (phy->rev >= 6) {
0526         b43legacy_phy_write(dev, 0x0801, backup[16]);
0527         b43legacy_phy_write(dev, 0x0060, backup[17]);
0528         b43legacy_phy_write(dev, 0x0014, backup[18]);
0529         b43legacy_phy_write(dev, 0x0478, backup[19]);
0530     }
0531     b43legacy_phy_write(dev, 0x0001, backup[0]);
0532     b43legacy_phy_write(dev, 0x0812, backup[2]);
0533     b43legacy_phy_write(dev, 0x0811, backup[1]);
0534 }
0535 
0536 void b43legacy_calc_nrssi_slope(struct b43legacy_wldev *dev)
0537 {
0538     struct b43legacy_phy *phy = &dev->phy;
0539     u16 backup[18] = { 0 };
0540     u16 tmp;
0541     s16 nrssi0;
0542     s16 nrssi1;
0543 
0544     switch (phy->type) {
0545     case B43legacy_PHYTYPE_B:
0546         backup[0] = b43legacy_radio_read16(dev, 0x007A);
0547         backup[1] = b43legacy_radio_read16(dev, 0x0052);
0548         backup[2] = b43legacy_radio_read16(dev, 0x0043);
0549         backup[3] = b43legacy_phy_read(dev, 0x0030);
0550         backup[4] = b43legacy_phy_read(dev, 0x0026);
0551         backup[5] = b43legacy_phy_read(dev, 0x0015);
0552         backup[6] = b43legacy_phy_read(dev, 0x002A);
0553         backup[7] = b43legacy_phy_read(dev, 0x0020);
0554         backup[8] = b43legacy_phy_read(dev, 0x005A);
0555         backup[9] = b43legacy_phy_read(dev, 0x0059);
0556         backup[10] = b43legacy_phy_read(dev, 0x0058);
0557         backup[11] = b43legacy_read16(dev, 0x03E2);
0558         backup[12] = b43legacy_read16(dev, 0x03E6);
0559         backup[13] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
0560 
0561         tmp  = b43legacy_radio_read16(dev, 0x007A);
0562         tmp &= (phy->rev >= 5) ? 0x007F : 0x000F;
0563         b43legacy_radio_write16(dev, 0x007A, tmp);
0564         b43legacy_phy_write(dev, 0x0030, 0x00FF);
0565         b43legacy_write16(dev, 0x03EC, 0x7F7F);
0566         b43legacy_phy_write(dev, 0x0026, 0x0000);
0567         b43legacy_phy_write(dev, 0x0015,
0568                     b43legacy_phy_read(dev, 0x0015) | 0x0020);
0569         b43legacy_phy_write(dev, 0x002A, 0x08A3);
0570         b43legacy_radio_write16(dev, 0x007A,
0571                     b43legacy_radio_read16(dev, 0x007A)
0572                     | 0x0080);
0573 
0574         nrssi0 = (s16)b43legacy_phy_read(dev, 0x0027);
0575         b43legacy_radio_write16(dev, 0x007A,
0576                     b43legacy_radio_read16(dev, 0x007A)
0577                     & 0x007F);
0578         if (phy->analog >= 2)
0579             b43legacy_write16(dev, 0x03E6, 0x0040);
0580         else if (phy->analog == 0)
0581             b43legacy_write16(dev, 0x03E6, 0x0122);
0582         else
0583             b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
0584                       b43legacy_read16(dev,
0585                       B43legacy_MMIO_CHANNEL_EXT) & 0x2000);
0586         b43legacy_phy_write(dev, 0x0020, 0x3F3F);
0587         b43legacy_phy_write(dev, 0x0015, 0xF330);
0588         b43legacy_radio_write16(dev, 0x005A, 0x0060);
0589         b43legacy_radio_write16(dev, 0x0043,
0590                     b43legacy_radio_read16(dev, 0x0043)
0591                     & 0x00F0);
0592         b43legacy_phy_write(dev, 0x005A, 0x0480);
0593         b43legacy_phy_write(dev, 0x0059, 0x0810);
0594         b43legacy_phy_write(dev, 0x0058, 0x000D);
0595         udelay(20);
0596 
0597         nrssi1 = (s16)b43legacy_phy_read(dev, 0x0027);
0598         b43legacy_phy_write(dev, 0x0030, backup[3]);
0599         b43legacy_radio_write16(dev, 0x007A, backup[0]);
0600         b43legacy_write16(dev, 0x03E2, backup[11]);
0601         b43legacy_phy_write(dev, 0x0026, backup[4]);
0602         b43legacy_phy_write(dev, 0x0015, backup[5]);
0603         b43legacy_phy_write(dev, 0x002A, backup[6]);
0604         b43legacy_synth_pu_workaround(dev, phy->channel);
0605         if (phy->analog != 0)
0606             b43legacy_write16(dev, 0x03F4, backup[13]);
0607 
0608         b43legacy_phy_write(dev, 0x0020, backup[7]);
0609         b43legacy_phy_write(dev, 0x005A, backup[8]);
0610         b43legacy_phy_write(dev, 0x0059, backup[9]);
0611         b43legacy_phy_write(dev, 0x0058, backup[10]);
0612         b43legacy_radio_write16(dev, 0x0052, backup[1]);
0613         b43legacy_radio_write16(dev, 0x0043, backup[2]);
0614 
0615         if (nrssi0 == nrssi1)
0616             phy->nrssislope = 0x00010000;
0617         else
0618             phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
0619 
0620         if (nrssi0 <= -4) {
0621             phy->nrssi[0] = nrssi0;
0622             phy->nrssi[1] = nrssi1;
0623         }
0624         break;
0625     case B43legacy_PHYTYPE_G:
0626         if (phy->radio_rev >= 9)
0627             return;
0628         if (phy->radio_rev == 8)
0629             b43legacy_calc_nrssi_offset(dev);
0630 
0631         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
0632                     b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
0633                     & 0x7FFF);
0634         b43legacy_phy_write(dev, 0x0802,
0635                     b43legacy_phy_read(dev, 0x0802) & 0xFFFC);
0636         backup[7] = b43legacy_read16(dev, 0x03E2);
0637         b43legacy_write16(dev, 0x03E2,
0638                   b43legacy_read16(dev, 0x03E2) | 0x8000);
0639         backup[0] = b43legacy_radio_read16(dev, 0x007A);
0640         backup[1] = b43legacy_radio_read16(dev, 0x0052);
0641         backup[2] = b43legacy_radio_read16(dev, 0x0043);
0642         backup[3] = b43legacy_phy_read(dev, 0x0015);
0643         backup[4] = b43legacy_phy_read(dev, 0x005A);
0644         backup[5] = b43legacy_phy_read(dev, 0x0059);
0645         backup[6] = b43legacy_phy_read(dev, 0x0058);
0646         backup[8] = b43legacy_read16(dev, 0x03E6);
0647         backup[9] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
0648         if (phy->rev >= 3) {
0649             backup[10] = b43legacy_phy_read(dev, 0x002E);
0650             backup[11] = b43legacy_phy_read(dev, 0x002F);
0651             backup[12] = b43legacy_phy_read(dev, 0x080F);
0652             backup[13] = b43legacy_phy_read(dev,
0653                         B43legacy_PHY_G_LO_CONTROL);
0654             backup[14] = b43legacy_phy_read(dev, 0x0801);
0655             backup[15] = b43legacy_phy_read(dev, 0x0060);
0656             backup[16] = b43legacy_phy_read(dev, 0x0014);
0657             backup[17] = b43legacy_phy_read(dev, 0x0478);
0658             b43legacy_phy_write(dev, 0x002E, 0);
0659             b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL, 0);
0660             switch (phy->rev) {
0661             case 4: case 6: case 7:
0662                 b43legacy_phy_write(dev, 0x0478,
0663                             b43legacy_phy_read(dev,
0664                             0x0478) | 0x0100);
0665                 b43legacy_phy_write(dev, 0x0801,
0666                             b43legacy_phy_read(dev,
0667                             0x0801) | 0x0040);
0668                 break;
0669             case 3: case 5:
0670                 b43legacy_phy_write(dev, 0x0801,
0671                             b43legacy_phy_read(dev,
0672                             0x0801) & 0xFFBF);
0673                 break;
0674             }
0675             b43legacy_phy_write(dev, 0x0060,
0676                         b43legacy_phy_read(dev, 0x0060)
0677                         | 0x0040);
0678             b43legacy_phy_write(dev, 0x0014,
0679                         b43legacy_phy_read(dev, 0x0014)
0680                         | 0x0200);
0681         }
0682         b43legacy_radio_write16(dev, 0x007A,
0683                     b43legacy_radio_read16(dev, 0x007A)
0684                     | 0x0070);
0685         b43legacy_set_all_gains(dev, 0, 8, 0);
0686         b43legacy_radio_write16(dev, 0x007A,
0687                     b43legacy_radio_read16(dev, 0x007A)
0688                     & 0x00F7);
0689         if (phy->rev >= 2) {
0690             b43legacy_phy_write(dev, 0x0811,
0691                         (b43legacy_phy_read(dev, 0x0811)
0692                         & 0xFFCF) | 0x0030);
0693             b43legacy_phy_write(dev, 0x0812,
0694                         (b43legacy_phy_read(dev, 0x0812)
0695                         & 0xFFCF) | 0x0010);
0696         }
0697         b43legacy_radio_write16(dev, 0x007A,
0698                     b43legacy_radio_read16(dev, 0x007A)
0699                     | 0x0080);
0700         udelay(20);
0701 
0702         nrssi0 = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
0703         if (nrssi0 >= 0x0020)
0704             nrssi0 -= 0x0040;
0705 
0706         b43legacy_radio_write16(dev, 0x007A,
0707                     b43legacy_radio_read16(dev, 0x007A)
0708                     & 0x007F);
0709         if (phy->analog >= 2)
0710             b43legacy_phy_write(dev, 0x0003,
0711                         (b43legacy_phy_read(dev, 0x0003)
0712                         & 0xFF9F) | 0x0040);
0713 
0714         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
0715                   b43legacy_read16(dev,
0716                   B43legacy_MMIO_CHANNEL_EXT) | 0x2000);
0717         b43legacy_radio_write16(dev, 0x007A,
0718                     b43legacy_radio_read16(dev, 0x007A)
0719                     | 0x000F);
0720         b43legacy_phy_write(dev, 0x0015, 0xF330);
0721         if (phy->rev >= 2) {
0722             b43legacy_phy_write(dev, 0x0812,
0723                         (b43legacy_phy_read(dev, 0x0812)
0724                         & 0xFFCF) | 0x0020);
0725             b43legacy_phy_write(dev, 0x0811,
0726                         (b43legacy_phy_read(dev, 0x0811)
0727                         & 0xFFCF) | 0x0020);
0728         }
0729 
0730         b43legacy_set_all_gains(dev, 3, 0, 1);
0731         if (phy->radio_rev == 8)
0732             b43legacy_radio_write16(dev, 0x0043, 0x001F);
0733         else {
0734             tmp = b43legacy_radio_read16(dev, 0x0052) & 0xFF0F;
0735             b43legacy_radio_write16(dev, 0x0052, tmp | 0x0060);
0736             tmp = b43legacy_radio_read16(dev, 0x0043) & 0xFFF0;
0737             b43legacy_radio_write16(dev, 0x0043, tmp | 0x0009);
0738         }
0739         b43legacy_phy_write(dev, 0x005A, 0x0480);
0740         b43legacy_phy_write(dev, 0x0059, 0x0810);
0741         b43legacy_phy_write(dev, 0x0058, 0x000D);
0742         udelay(20);
0743         nrssi1 = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
0744         if (nrssi1 >= 0x0020)
0745             nrssi1 -= 0x0040;
0746         if (nrssi0 == nrssi1)
0747             phy->nrssislope = 0x00010000;
0748         else
0749             phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
0750         if (nrssi0 >= -4) {
0751             phy->nrssi[0] = nrssi1;
0752             phy->nrssi[1] = nrssi0;
0753         }
0754         if (phy->rev >= 3) {
0755             b43legacy_phy_write(dev, 0x002E, backup[10]);
0756             b43legacy_phy_write(dev, 0x002F, backup[11]);
0757             b43legacy_phy_write(dev, 0x080F, backup[12]);
0758             b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL,
0759                         backup[13]);
0760         }
0761         if (phy->rev >= 2) {
0762             b43legacy_phy_write(dev, 0x0812,
0763                         b43legacy_phy_read(dev, 0x0812)
0764                         & 0xFFCF);
0765             b43legacy_phy_write(dev, 0x0811,
0766                         b43legacy_phy_read(dev, 0x0811)
0767                         & 0xFFCF);
0768         }
0769 
0770         b43legacy_radio_write16(dev, 0x007A, backup[0]);
0771         b43legacy_radio_write16(dev, 0x0052, backup[1]);
0772         b43legacy_radio_write16(dev, 0x0043, backup[2]);
0773         b43legacy_write16(dev, 0x03E2, backup[7]);
0774         b43legacy_write16(dev, 0x03E6, backup[8]);
0775         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, backup[9]);
0776         b43legacy_phy_write(dev, 0x0015, backup[3]);
0777         b43legacy_phy_write(dev, 0x005A, backup[4]);
0778         b43legacy_phy_write(dev, 0x0059, backup[5]);
0779         b43legacy_phy_write(dev, 0x0058, backup[6]);
0780         b43legacy_synth_pu_workaround(dev, phy->channel);
0781         b43legacy_phy_write(dev, 0x0802,
0782                     b43legacy_phy_read(dev, 0x0802) | 0x0003);
0783         b43legacy_set_original_gains(dev);
0784         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
0785                     b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
0786                     | 0x8000);
0787         if (phy->rev >= 3) {
0788             b43legacy_phy_write(dev, 0x0801, backup[14]);
0789             b43legacy_phy_write(dev, 0x0060, backup[15]);
0790             b43legacy_phy_write(dev, 0x0014, backup[16]);
0791             b43legacy_phy_write(dev, 0x0478, backup[17]);
0792         }
0793         b43legacy_nrssi_mem_update(dev);
0794         b43legacy_calc_nrssi_threshold(dev);
0795         break;
0796     default:
0797         B43legacy_BUG_ON(1);
0798     }
0799 }
0800 
0801 void b43legacy_calc_nrssi_threshold(struct b43legacy_wldev *dev)
0802 {
0803     struct b43legacy_phy *phy = &dev->phy;
0804     s32 threshold;
0805     s32 a;
0806     s32 b;
0807     s16 tmp16;
0808     u16 tmp_u16;
0809 
0810     switch (phy->type) {
0811     case B43legacy_PHYTYPE_B: {
0812         if (phy->radio_ver != 0x2050)
0813             return;
0814         if (!(dev->dev->bus->sprom.boardflags_lo &
0815             B43legacy_BFL_RSSI))
0816             return;
0817 
0818         if (phy->radio_rev >= 6) {
0819             threshold = (phy->nrssi[1] - phy->nrssi[0]) * 32;
0820             threshold += 20 * (phy->nrssi[0] + 1);
0821             threshold /= 40;
0822         } else
0823             threshold = phy->nrssi[1] - 5;
0824 
0825         threshold = clamp_val(threshold, 0, 0x3E);
0826         b43legacy_phy_read(dev, 0x0020); /* dummy read */
0827         b43legacy_phy_write(dev, 0x0020, (((u16)threshold) << 8)
0828                     | 0x001C);
0829 
0830         if (phy->radio_rev >= 6) {
0831             b43legacy_phy_write(dev, 0x0087, 0x0E0D);
0832             b43legacy_phy_write(dev, 0x0086, 0x0C0B);
0833             b43legacy_phy_write(dev, 0x0085, 0x0A09);
0834             b43legacy_phy_write(dev, 0x0084, 0x0808);
0835             b43legacy_phy_write(dev, 0x0083, 0x0808);
0836             b43legacy_phy_write(dev, 0x0082, 0x0604);
0837             b43legacy_phy_write(dev, 0x0081, 0x0302);
0838             b43legacy_phy_write(dev, 0x0080, 0x0100);
0839         }
0840         break;
0841     }
0842     case B43legacy_PHYTYPE_G:
0843         if (!phy->gmode ||
0844             !(dev->dev->bus->sprom.boardflags_lo &
0845             B43legacy_BFL_RSSI)) {
0846             tmp16 = b43legacy_nrssi_hw_read(dev, 0x20);
0847             if (tmp16 >= 0x20)
0848                 tmp16 -= 0x40;
0849             if (tmp16 < 3)
0850                 b43legacy_phy_write(dev, 0x048A,
0851                             (b43legacy_phy_read(dev,
0852                             0x048A) & 0xF000) | 0x09EB);
0853             else
0854                 b43legacy_phy_write(dev, 0x048A,
0855                             (b43legacy_phy_read(dev,
0856                             0x048A) & 0xF000) | 0x0AED);
0857         } else {
0858             if (phy->interfmode ==
0859                 B43legacy_RADIO_INTERFMODE_NONWLAN) {
0860                 a = 0xE;
0861                 b = 0xA;
0862             } else if (!phy->aci_wlan_automatic &&
0863                     phy->aci_enable) {
0864                 a = 0x13;
0865                 b = 0x12;
0866             } else {
0867                 a = 0xE;
0868                 b = 0x11;
0869             }
0870 
0871             a = a * (phy->nrssi[1] - phy->nrssi[0]);
0872             a += (phy->nrssi[0] << 6);
0873             if (a < 32)
0874                 a += 31;
0875             else
0876                 a += 32;
0877             a = a >> 6;
0878             a = clamp_val(a, -31, 31);
0879 
0880             b = b * (phy->nrssi[1] - phy->nrssi[0]);
0881             b += (phy->nrssi[0] << 6);
0882             if (b < 32)
0883                 b += 31;
0884             else
0885                 b += 32;
0886             b = b >> 6;
0887             b = clamp_val(b, -31, 31);
0888 
0889             tmp_u16 = b43legacy_phy_read(dev, 0x048A) & 0xF000;
0890             tmp_u16 |= ((u32)b & 0x0000003F);
0891             tmp_u16 |= (((u32)a & 0x0000003F) << 6);
0892             b43legacy_phy_write(dev, 0x048A, tmp_u16);
0893         }
0894         break;
0895     default:
0896         B43legacy_BUG_ON(1);
0897     }
0898 }
0899 
0900 /* Stack implementation to save/restore values from the
0901  * interference mitigation code.
0902  * It is save to restore values in random order.
0903  */
0904 static void _stack_save(u32 *_stackptr, size_t *stackidx,
0905             u8 id, u16 offset, u16 value)
0906 {
0907     u32 *stackptr = &(_stackptr[*stackidx]);
0908 
0909     B43legacy_WARN_ON(!((offset & 0xE000) == 0x0000));
0910     B43legacy_WARN_ON(!((id & 0xF8) == 0x00));
0911     *stackptr = offset;
0912     *stackptr |= ((u32)id) << 13;
0913     *stackptr |= ((u32)value) << 16;
0914     (*stackidx)++;
0915     B43legacy_WARN_ON(!(*stackidx < B43legacy_INTERFSTACK_SIZE));
0916 }
0917 
0918 static u16 _stack_restore(u32 *stackptr,
0919               u8 id, u16 offset)
0920 {
0921     size_t i;
0922 
0923     B43legacy_WARN_ON(!((offset & 0xE000) == 0x0000));
0924     B43legacy_WARN_ON(!((id & 0xF8) == 0x00));
0925     for (i = 0; i < B43legacy_INTERFSTACK_SIZE; i++, stackptr++) {
0926         if ((*stackptr & 0x00001FFF) != offset)
0927             continue;
0928         if (((*stackptr & 0x00007000) >> 13) != id)
0929             continue;
0930         return ((*stackptr & 0xFFFF0000) >> 16);
0931     }
0932     B43legacy_BUG_ON(1);
0933 
0934     return 0;
0935 }
0936 
0937 #define phy_stacksave(offset)                   \
0938     do {                            \
0939         _stack_save(stack, &stackidx, 0x1, (offset),    \
0940                 b43legacy_phy_read(dev, (offset))); \
0941     } while (0)
0942 #define phy_stackrestore(offset)                \
0943     do {                            \
0944         b43legacy_phy_write(dev, (offset),      \
0945                     _stack_restore(stack, 0x1,  \
0946                     (offset)));         \
0947     } while (0)
0948 #define radio_stacksave(offset)                     \
0949     do {                                \
0950         _stack_save(stack, &stackidx, 0x2, (offset),        \
0951                 b43legacy_radio_read16(dev, (offset))); \
0952     } while (0)
0953 #define radio_stackrestore(offset)                  \
0954     do {                                \
0955         b43legacy_radio_write16(dev, (offset),          \
0956                     _stack_restore(stack, 0x2,  \
0957                     (offset)));         \
0958     } while (0)
0959 #define ilt_stacksave(offset)                   \
0960     do {                            \
0961         _stack_save(stack, &stackidx, 0x3, (offset),    \
0962                 b43legacy_ilt_read(dev, (offset))); \
0963     } while (0)
0964 #define ilt_stackrestore(offset)                \
0965     do {                            \
0966         b43legacy_ilt_write(dev, (offset),      \
0967                   _stack_restore(stack, 0x3,    \
0968                          (offset)));    \
0969     } while (0)
0970 
0971 static void
0972 b43legacy_radio_interference_mitigation_enable(struct b43legacy_wldev *dev,
0973                            int mode)
0974 {
0975     struct b43legacy_phy *phy = &dev->phy;
0976     u16 tmp;
0977     u16 flipped;
0978     u32 tmp32;
0979     size_t stackidx = 0;
0980     u32 *stack = phy->interfstack;
0981 
0982     switch (mode) {
0983     case B43legacy_RADIO_INTERFMODE_NONWLAN:
0984         if (phy->rev != 1) {
0985             b43legacy_phy_write(dev, 0x042B,
0986                         b43legacy_phy_read(dev, 0x042B)
0987                         | 0x0800);
0988             b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
0989                         b43legacy_phy_read(dev,
0990                         B43legacy_PHY_G_CRS) & ~0x4000);
0991             break;
0992         }
0993         radio_stacksave(0x0078);
0994         tmp = (b43legacy_radio_read16(dev, 0x0078) & 0x001E);
0995         flipped = flip_4bit(tmp);
0996         if (flipped < 10 && flipped >= 8)
0997             flipped = 7;
0998         else if (flipped >= 10)
0999             flipped -= 3;
1000         flipped = flip_4bit(flipped);
1001         flipped = (flipped << 1) | 0x0020;
1002         b43legacy_radio_write16(dev, 0x0078, flipped);
1003 
1004         b43legacy_calc_nrssi_threshold(dev);
1005 
1006         phy_stacksave(0x0406);
1007         b43legacy_phy_write(dev, 0x0406, 0x7E28);
1008 
1009         b43legacy_phy_write(dev, 0x042B,
1010                     b43legacy_phy_read(dev, 0x042B) | 0x0800);
1011         b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
1012                     b43legacy_phy_read(dev,
1013                     B43legacy_PHY_RADIO_BITFIELD) | 0x1000);
1014 
1015         phy_stacksave(0x04A0);
1016         b43legacy_phy_write(dev, 0x04A0,
1017                     (b43legacy_phy_read(dev, 0x04A0) & 0xC0C0)
1018                     | 0x0008);
1019         phy_stacksave(0x04A1);
1020         b43legacy_phy_write(dev, 0x04A1,
1021                     (b43legacy_phy_read(dev, 0x04A1) & 0xC0C0)
1022                     | 0x0605);
1023         phy_stacksave(0x04A2);
1024         b43legacy_phy_write(dev, 0x04A2,
1025                     (b43legacy_phy_read(dev, 0x04A2) & 0xC0C0)
1026                     | 0x0204);
1027         phy_stacksave(0x04A8);
1028         b43legacy_phy_write(dev, 0x04A8,
1029                     (b43legacy_phy_read(dev, 0x04A8) & 0xC0C0)
1030                     | 0x0803);
1031         phy_stacksave(0x04AB);
1032         b43legacy_phy_write(dev, 0x04AB,
1033                     (b43legacy_phy_read(dev, 0x04AB) & 0xC0C0)
1034                     | 0x0605);
1035 
1036         phy_stacksave(0x04A7);
1037         b43legacy_phy_write(dev, 0x04A7, 0x0002);
1038         phy_stacksave(0x04A3);
1039         b43legacy_phy_write(dev, 0x04A3, 0x287A);
1040         phy_stacksave(0x04A9);
1041         b43legacy_phy_write(dev, 0x04A9, 0x2027);
1042         phy_stacksave(0x0493);
1043         b43legacy_phy_write(dev, 0x0493, 0x32F5);
1044         phy_stacksave(0x04AA);
1045         b43legacy_phy_write(dev, 0x04AA, 0x2027);
1046         phy_stacksave(0x04AC);
1047         b43legacy_phy_write(dev, 0x04AC, 0x32F5);
1048         break;
1049     case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
1050         if (b43legacy_phy_read(dev, 0x0033) & 0x0800)
1051             break;
1052 
1053         phy->aci_enable = true;
1054 
1055         phy_stacksave(B43legacy_PHY_RADIO_BITFIELD);
1056         phy_stacksave(B43legacy_PHY_G_CRS);
1057         if (phy->rev < 2)
1058             phy_stacksave(0x0406);
1059         else {
1060             phy_stacksave(0x04C0);
1061             phy_stacksave(0x04C1);
1062         }
1063         phy_stacksave(0x0033);
1064         phy_stacksave(0x04A7);
1065         phy_stacksave(0x04A3);
1066         phy_stacksave(0x04A9);
1067         phy_stacksave(0x04AA);
1068         phy_stacksave(0x04AC);
1069         phy_stacksave(0x0493);
1070         phy_stacksave(0x04A1);
1071         phy_stacksave(0x04A0);
1072         phy_stacksave(0x04A2);
1073         phy_stacksave(0x048A);
1074         phy_stacksave(0x04A8);
1075         phy_stacksave(0x04AB);
1076         if (phy->rev == 2) {
1077             phy_stacksave(0x04AD);
1078             phy_stacksave(0x04AE);
1079         } else if (phy->rev >= 3) {
1080             phy_stacksave(0x04AD);
1081             phy_stacksave(0x0415);
1082             phy_stacksave(0x0416);
1083             phy_stacksave(0x0417);
1084             ilt_stacksave(0x1A00 + 0x2);
1085             ilt_stacksave(0x1A00 + 0x3);
1086         }
1087         phy_stacksave(0x042B);
1088         phy_stacksave(0x048C);
1089 
1090         b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
1091                     b43legacy_phy_read(dev,
1092                     B43legacy_PHY_RADIO_BITFIELD) & ~0x1000);
1093         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1094                     (b43legacy_phy_read(dev,
1095                     B43legacy_PHY_G_CRS)
1096                     & 0xFFFC) | 0x0002);
1097 
1098         b43legacy_phy_write(dev, 0x0033, 0x0800);
1099         b43legacy_phy_write(dev, 0x04A3, 0x2027);
1100         b43legacy_phy_write(dev, 0x04A9, 0x1CA8);
1101         b43legacy_phy_write(dev, 0x0493, 0x287A);
1102         b43legacy_phy_write(dev, 0x04AA, 0x1CA8);
1103         b43legacy_phy_write(dev, 0x04AC, 0x287A);
1104 
1105         b43legacy_phy_write(dev, 0x04A0,
1106                     (b43legacy_phy_read(dev, 0x04A0)
1107                     & 0xFFC0) | 0x001A);
1108         b43legacy_phy_write(dev, 0x04A7, 0x000D);
1109 
1110         if (phy->rev < 2)
1111             b43legacy_phy_write(dev, 0x0406, 0xFF0D);
1112         else if (phy->rev == 2) {
1113             b43legacy_phy_write(dev, 0x04C0, 0xFFFF);
1114             b43legacy_phy_write(dev, 0x04C1, 0x00A9);
1115         } else {
1116             b43legacy_phy_write(dev, 0x04C0, 0x00C1);
1117             b43legacy_phy_write(dev, 0x04C1, 0x0059);
1118         }
1119 
1120         b43legacy_phy_write(dev, 0x04A1,
1121                     (b43legacy_phy_read(dev, 0x04A1)
1122                     & 0xC0FF) | 0x1800);
1123         b43legacy_phy_write(dev, 0x04A1,
1124                     (b43legacy_phy_read(dev, 0x04A1)
1125                     & 0xFFC0) | 0x0015);
1126         b43legacy_phy_write(dev, 0x04A8,
1127                     (b43legacy_phy_read(dev, 0x04A8)
1128                     & 0xCFFF) | 0x1000);
1129         b43legacy_phy_write(dev, 0x04A8,
1130                     (b43legacy_phy_read(dev, 0x04A8)
1131                     & 0xF0FF) | 0x0A00);
1132         b43legacy_phy_write(dev, 0x04AB,
1133                     (b43legacy_phy_read(dev, 0x04AB)
1134                     & 0xCFFF) | 0x1000);
1135         b43legacy_phy_write(dev, 0x04AB,
1136                     (b43legacy_phy_read(dev, 0x04AB)
1137                     & 0xF0FF) | 0x0800);
1138         b43legacy_phy_write(dev, 0x04AB,
1139                     (b43legacy_phy_read(dev, 0x04AB)
1140                     & 0xFFCF) | 0x0010);
1141         b43legacy_phy_write(dev, 0x04AB,
1142                     (b43legacy_phy_read(dev, 0x04AB)
1143                     & 0xFFF0) | 0x0005);
1144         b43legacy_phy_write(dev, 0x04A8,
1145                     (b43legacy_phy_read(dev, 0x04A8)
1146                     & 0xFFCF) | 0x0010);
1147         b43legacy_phy_write(dev, 0x04A8,
1148                     (b43legacy_phy_read(dev, 0x04A8)
1149                     & 0xFFF0) | 0x0006);
1150         b43legacy_phy_write(dev, 0x04A2,
1151                     (b43legacy_phy_read(dev, 0x04A2)
1152                     & 0xF0FF) | 0x0800);
1153         b43legacy_phy_write(dev, 0x04A0,
1154                     (b43legacy_phy_read(dev, 0x04A0)
1155                     & 0xF0FF) | 0x0500);
1156         b43legacy_phy_write(dev, 0x04A2,
1157                     (b43legacy_phy_read(dev, 0x04A2)
1158                     & 0xFFF0) | 0x000B);
1159 
1160         if (phy->rev >= 3) {
1161             b43legacy_phy_write(dev, 0x048A,
1162                         b43legacy_phy_read(dev, 0x048A)
1163                         & ~0x8000);
1164             b43legacy_phy_write(dev, 0x0415,
1165                         (b43legacy_phy_read(dev, 0x0415)
1166                         & 0x8000) | 0x36D8);
1167             b43legacy_phy_write(dev, 0x0416,
1168                         (b43legacy_phy_read(dev, 0x0416)
1169                         & 0x8000) | 0x36D8);
1170             b43legacy_phy_write(dev, 0x0417,
1171                         (b43legacy_phy_read(dev, 0x0417)
1172                         & 0xFE00) | 0x016D);
1173         } else {
1174             b43legacy_phy_write(dev, 0x048A,
1175                         b43legacy_phy_read(dev, 0x048A)
1176                         | 0x1000);
1177             b43legacy_phy_write(dev, 0x048A,
1178                         (b43legacy_phy_read(dev, 0x048A)
1179                         & 0x9FFF) | 0x2000);
1180             tmp32 = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
1181                         B43legacy_UCODEFLAGS_OFFSET);
1182             if (!(tmp32 & 0x800)) {
1183                 tmp32 |= 0x800;
1184                 b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
1185                         B43legacy_UCODEFLAGS_OFFSET,
1186                         tmp32);
1187             }
1188         }
1189         if (phy->rev >= 2)
1190             b43legacy_phy_write(dev, 0x042B,
1191                         b43legacy_phy_read(dev, 0x042B)
1192                         | 0x0800);
1193         b43legacy_phy_write(dev, 0x048C,
1194                     (b43legacy_phy_read(dev, 0x048C)
1195                     & 0xF0FF) | 0x0200);
1196         if (phy->rev == 2) {
1197             b43legacy_phy_write(dev, 0x04AE,
1198                         (b43legacy_phy_read(dev, 0x04AE)
1199                         & 0xFF00) | 0x007F);
1200             b43legacy_phy_write(dev, 0x04AD,
1201                         (b43legacy_phy_read(dev, 0x04AD)
1202                         & 0x00FF) | 0x1300);
1203         } else if (phy->rev >= 6) {
1204             b43legacy_ilt_write(dev, 0x1A00 + 0x3, 0x007F);
1205             b43legacy_ilt_write(dev, 0x1A00 + 0x2, 0x007F);
1206             b43legacy_phy_write(dev, 0x04AD,
1207                         b43legacy_phy_read(dev, 0x04AD)
1208                         & 0x00FF);
1209         }
1210         b43legacy_calc_nrssi_slope(dev);
1211         break;
1212     default:
1213         B43legacy_BUG_ON(1);
1214     }
1215 }
1216 
1217 static void
1218 b43legacy_radio_interference_mitigation_disable(struct b43legacy_wldev *dev,
1219                         int mode)
1220 {
1221     struct b43legacy_phy *phy = &dev->phy;
1222     u32 tmp32;
1223     u32 *stack = phy->interfstack;
1224 
1225     switch (mode) {
1226     case B43legacy_RADIO_INTERFMODE_NONWLAN:
1227         if (phy->rev != 1) {
1228             b43legacy_phy_write(dev, 0x042B,
1229                         b43legacy_phy_read(dev, 0x042B)
1230                         & ~0x0800);
1231             b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1232                         b43legacy_phy_read(dev,
1233                         B43legacy_PHY_G_CRS) | 0x4000);
1234             break;
1235         }
1236         phy_stackrestore(0x0078);
1237         b43legacy_calc_nrssi_threshold(dev);
1238         phy_stackrestore(0x0406);
1239         b43legacy_phy_write(dev, 0x042B,
1240                     b43legacy_phy_read(dev, 0x042B) & ~0x0800);
1241         if (!dev->bad_frames_preempt)
1242             b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
1243                         b43legacy_phy_read(dev,
1244                         B43legacy_PHY_RADIO_BITFIELD)
1245                         & ~(1 << 11));
1246         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1247                     b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
1248                     | 0x4000);
1249         phy_stackrestore(0x04A0);
1250         phy_stackrestore(0x04A1);
1251         phy_stackrestore(0x04A2);
1252         phy_stackrestore(0x04A8);
1253         phy_stackrestore(0x04AB);
1254         phy_stackrestore(0x04A7);
1255         phy_stackrestore(0x04A3);
1256         phy_stackrestore(0x04A9);
1257         phy_stackrestore(0x0493);
1258         phy_stackrestore(0x04AA);
1259         phy_stackrestore(0x04AC);
1260         break;
1261     case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
1262         if (!(b43legacy_phy_read(dev, 0x0033) & 0x0800))
1263             break;
1264 
1265         phy->aci_enable = false;
1266 
1267         phy_stackrestore(B43legacy_PHY_RADIO_BITFIELD);
1268         phy_stackrestore(B43legacy_PHY_G_CRS);
1269         phy_stackrestore(0x0033);
1270         phy_stackrestore(0x04A3);
1271         phy_stackrestore(0x04A9);
1272         phy_stackrestore(0x0493);
1273         phy_stackrestore(0x04AA);
1274         phy_stackrestore(0x04AC);
1275         phy_stackrestore(0x04A0);
1276         phy_stackrestore(0x04A7);
1277         if (phy->rev >= 2) {
1278             phy_stackrestore(0x04C0);
1279             phy_stackrestore(0x04C1);
1280         } else
1281             phy_stackrestore(0x0406);
1282         phy_stackrestore(0x04A1);
1283         phy_stackrestore(0x04AB);
1284         phy_stackrestore(0x04A8);
1285         if (phy->rev == 2) {
1286             phy_stackrestore(0x04AD);
1287             phy_stackrestore(0x04AE);
1288         } else if (phy->rev >= 3) {
1289             phy_stackrestore(0x04AD);
1290             phy_stackrestore(0x0415);
1291             phy_stackrestore(0x0416);
1292             phy_stackrestore(0x0417);
1293             ilt_stackrestore(0x1A00 + 0x2);
1294             ilt_stackrestore(0x1A00 + 0x3);
1295         }
1296         phy_stackrestore(0x04A2);
1297         phy_stackrestore(0x04A8);
1298         phy_stackrestore(0x042B);
1299         phy_stackrestore(0x048C);
1300         tmp32 = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
1301                          B43legacy_UCODEFLAGS_OFFSET);
1302         if (tmp32 & 0x800) {
1303             tmp32 &= ~0x800;
1304             b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
1305                           B43legacy_UCODEFLAGS_OFFSET,
1306                           tmp32);
1307         }
1308         b43legacy_calc_nrssi_slope(dev);
1309         break;
1310     default:
1311         B43legacy_BUG_ON(1);
1312     }
1313 }
1314 
1315 #undef phy_stacksave
1316 #undef phy_stackrestore
1317 #undef radio_stacksave
1318 #undef radio_stackrestore
1319 #undef ilt_stacksave
1320 #undef ilt_stackrestore
1321 
1322 int b43legacy_radio_set_interference_mitigation(struct b43legacy_wldev *dev,
1323                         int mode)
1324 {
1325     struct b43legacy_phy *phy = &dev->phy;
1326     int currentmode;
1327 
1328     if ((phy->type != B43legacy_PHYTYPE_G) ||
1329         (phy->rev == 0) || (!phy->gmode))
1330         return -ENODEV;
1331 
1332     phy->aci_wlan_automatic = false;
1333     switch (mode) {
1334     case B43legacy_RADIO_INTERFMODE_AUTOWLAN:
1335         phy->aci_wlan_automatic = true;
1336         if (phy->aci_enable)
1337             mode = B43legacy_RADIO_INTERFMODE_MANUALWLAN;
1338         else
1339             mode = B43legacy_RADIO_INTERFMODE_NONE;
1340         break;
1341     case B43legacy_RADIO_INTERFMODE_NONE:
1342     case B43legacy_RADIO_INTERFMODE_NONWLAN:
1343     case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
1344         break;
1345     default:
1346         return -EINVAL;
1347     }
1348 
1349     currentmode = phy->interfmode;
1350     if (currentmode == mode)
1351         return 0;
1352     if (currentmode != B43legacy_RADIO_INTERFMODE_NONE)
1353         b43legacy_radio_interference_mitigation_disable(dev,
1354                                 currentmode);
1355 
1356     if (mode == B43legacy_RADIO_INTERFMODE_NONE) {
1357         phy->aci_enable = false;
1358         phy->aci_hw_rssi = false;
1359     } else
1360         b43legacy_radio_interference_mitigation_enable(dev, mode);
1361     phy->interfmode = mode;
1362 
1363     return 0;
1364 }
1365 
1366 u16 b43legacy_radio_calibrationvalue(struct b43legacy_wldev *dev)
1367 {
1368     u16 reg;
1369     u16 index;
1370     u16 ret;
1371 
1372     reg = b43legacy_radio_read16(dev, 0x0060);
1373     index = (reg & 0x001E) >> 1;
1374     ret = rcc_table[index] << 1;
1375     ret |= (reg & 0x0001);
1376     ret |= 0x0020;
1377 
1378     return ret;
1379 }
1380 
1381 #define LPD(L, P, D)    (((L) << 2) | ((P) << 1) | ((D) << 0))
1382 static u16 b43legacy_get_812_value(struct b43legacy_wldev *dev, u8 lpd)
1383 {
1384     struct b43legacy_phy *phy = &dev->phy;
1385     u16 loop_or = 0;
1386     u16 adj_loopback_gain = phy->loopback_gain[0];
1387     u8 loop;
1388     u16 extern_lna_control;
1389 
1390     if (!phy->gmode)
1391         return 0;
1392     if (!has_loopback_gain(phy)) {
1393         if (phy->rev < 7 || !(dev->dev->bus->sprom.boardflags_lo
1394             & B43legacy_BFL_EXTLNA)) {
1395             switch (lpd) {
1396             case LPD(0, 1, 1):
1397                 return 0x0FB2;
1398             case LPD(0, 0, 1):
1399                 return 0x00B2;
1400             case LPD(1, 0, 1):
1401                 return 0x30B2;
1402             case LPD(1, 0, 0):
1403                 return 0x30B3;
1404             default:
1405                 B43legacy_BUG_ON(1);
1406             }
1407         } else {
1408             switch (lpd) {
1409             case LPD(0, 1, 1):
1410                 return 0x8FB2;
1411             case LPD(0, 0, 1):
1412                 return 0x80B2;
1413             case LPD(1, 0, 1):
1414                 return 0x20B2;
1415             case LPD(1, 0, 0):
1416                 return 0x20B3;
1417             default:
1418                 B43legacy_BUG_ON(1);
1419             }
1420         }
1421     } else {
1422         if (phy->radio_rev == 8)
1423             adj_loopback_gain += 0x003E;
1424         else
1425             adj_loopback_gain += 0x0026;
1426         if (adj_loopback_gain >= 0x46) {
1427             adj_loopback_gain -= 0x46;
1428             extern_lna_control = 0x3000;
1429         } else if (adj_loopback_gain >= 0x3A) {
1430             adj_loopback_gain -= 0x3A;
1431             extern_lna_control = 0x2000;
1432         } else if (adj_loopback_gain >= 0x2E) {
1433             adj_loopback_gain -= 0x2E;
1434             extern_lna_control = 0x1000;
1435         } else {
1436             adj_loopback_gain -= 0x10;
1437             extern_lna_control = 0x0000;
1438         }
1439         for (loop = 0; loop < 16; loop++) {
1440             u16 tmp = adj_loopback_gain - 6 * loop;
1441             if (tmp < 6)
1442                 break;
1443         }
1444 
1445         loop_or = (loop << 8) | extern_lna_control;
1446         if (phy->rev >= 7 && dev->dev->bus->sprom.boardflags_lo
1447             & B43legacy_BFL_EXTLNA) {
1448             if (extern_lna_control)
1449                 loop_or |= 0x8000;
1450             switch (lpd) {
1451             case LPD(0, 1, 1):
1452                 return 0x8F92;
1453             case LPD(0, 0, 1):
1454                 return (0x8092 | loop_or);
1455             case LPD(1, 0, 1):
1456                 return (0x2092 | loop_or);
1457             case LPD(1, 0, 0):
1458                 return (0x2093 | loop_or);
1459             default:
1460                 B43legacy_BUG_ON(1);
1461             }
1462         } else {
1463             switch (lpd) {
1464             case LPD(0, 1, 1):
1465                 return 0x0F92;
1466             case LPD(0, 0, 1):
1467             case LPD(1, 0, 1):
1468                 return (0x0092 | loop_or);
1469             case LPD(1, 0, 0):
1470                 return (0x0093 | loop_or);
1471             default:
1472                 B43legacy_BUG_ON(1);
1473             }
1474         }
1475     }
1476     return 0;
1477 }
1478 
1479 u16 b43legacy_radio_init2050(struct b43legacy_wldev *dev)
1480 {
1481     struct b43legacy_phy *phy = &dev->phy;
1482     u16 backup[21] = { 0 };
1483     u16 ret;
1484     u16 i;
1485     u16 j;
1486     u32 tmp1 = 0;
1487     u32 tmp2 = 0;
1488 
1489     backup[0] = b43legacy_radio_read16(dev, 0x0043);
1490     backup[14] = b43legacy_radio_read16(dev, 0x0051);
1491     backup[15] = b43legacy_radio_read16(dev, 0x0052);
1492     backup[1] = b43legacy_phy_read(dev, 0x0015);
1493     backup[16] = b43legacy_phy_read(dev, 0x005A);
1494     backup[17] = b43legacy_phy_read(dev, 0x0059);
1495     backup[18] = b43legacy_phy_read(dev, 0x0058);
1496     if (phy->type == B43legacy_PHYTYPE_B) {
1497         backup[2] = b43legacy_phy_read(dev, 0x0030);
1498         backup[3] = b43legacy_read16(dev, 0x03EC);
1499         b43legacy_phy_write(dev, 0x0030, 0x00FF);
1500         b43legacy_write16(dev, 0x03EC, 0x3F3F);
1501     } else {
1502         if (phy->gmode) {
1503             backup[4] = b43legacy_phy_read(dev, 0x0811);
1504             backup[5] = b43legacy_phy_read(dev, 0x0812);
1505             backup[6] = b43legacy_phy_read(dev, 0x0814);
1506             backup[7] = b43legacy_phy_read(dev, 0x0815);
1507             backup[8] = b43legacy_phy_read(dev,
1508                                B43legacy_PHY_G_CRS);
1509             backup[9] = b43legacy_phy_read(dev, 0x0802);
1510             b43legacy_phy_write(dev, 0x0814,
1511                         (b43legacy_phy_read(dev, 0x0814)
1512                         | 0x0003));
1513             b43legacy_phy_write(dev, 0x0815,
1514                         (b43legacy_phy_read(dev, 0x0815)
1515                         & 0xFFFC));
1516             b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1517                         (b43legacy_phy_read(dev,
1518                         B43legacy_PHY_G_CRS) & 0x7FFF));
1519             b43legacy_phy_write(dev, 0x0802,
1520                         (b43legacy_phy_read(dev, 0x0802)
1521                         & 0xFFFC));
1522             if (phy->rev > 1) { /* loopback gain enabled */
1523                 backup[19] = b43legacy_phy_read(dev, 0x080F);
1524                 backup[20] = b43legacy_phy_read(dev, 0x0810);
1525                 if (phy->rev >= 3)
1526                     b43legacy_phy_write(dev, 0x080F,
1527                                 0xC020);
1528                 else
1529                     b43legacy_phy_write(dev, 0x080F,
1530                                 0x8020);
1531                 b43legacy_phy_write(dev, 0x0810, 0x0000);
1532             }
1533             b43legacy_phy_write(dev, 0x0812,
1534                         b43legacy_get_812_value(dev,
1535                         LPD(0, 1, 1)));
1536             if (phy->rev < 7 ||
1537                 !(dev->dev->bus->sprom.boardflags_lo
1538                 & B43legacy_BFL_EXTLNA))
1539                 b43legacy_phy_write(dev, 0x0811, 0x01B3);
1540             else
1541                 b43legacy_phy_write(dev, 0x0811, 0x09B3);
1542         }
1543     }
1544     b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO,
1545             (b43legacy_read16(dev, B43legacy_MMIO_PHY_RADIO)
1546                       | 0x8000));
1547     backup[10] = b43legacy_phy_read(dev, 0x0035);
1548     b43legacy_phy_write(dev, 0x0035,
1549                 (b43legacy_phy_read(dev, 0x0035) & 0xFF7F));
1550     backup[11] = b43legacy_read16(dev, 0x03E6);
1551     backup[12] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
1552 
1553     /* Initialization */
1554     if (phy->analog == 0)
1555         b43legacy_write16(dev, 0x03E6, 0x0122);
1556     else {
1557         if (phy->analog >= 2)
1558             b43legacy_phy_write(dev, 0x0003,
1559                         (b43legacy_phy_read(dev, 0x0003)
1560                         & 0xFFBF) | 0x0040);
1561         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
1562                   (b43legacy_read16(dev,
1563                   B43legacy_MMIO_CHANNEL_EXT) | 0x2000));
1564     }
1565 
1566     ret = b43legacy_radio_calibrationvalue(dev);
1567 
1568     if (phy->type == B43legacy_PHYTYPE_B)
1569         b43legacy_radio_write16(dev, 0x0078, 0x0026);
1570 
1571     if (phy->gmode)
1572         b43legacy_phy_write(dev, 0x0812,
1573                     b43legacy_get_812_value(dev,
1574                     LPD(0, 1, 1)));
1575     b43legacy_phy_write(dev, 0x0015, 0xBFAF);
1576     b43legacy_phy_write(dev, 0x002B, 0x1403);
1577     if (phy->gmode)
1578         b43legacy_phy_write(dev, 0x0812,
1579                     b43legacy_get_812_value(dev,
1580                     LPD(0, 0, 1)));
1581     b43legacy_phy_write(dev, 0x0015, 0xBFA0);
1582     b43legacy_radio_write16(dev, 0x0051,
1583                 (b43legacy_radio_read16(dev, 0x0051)
1584                 | 0x0004));
1585     if (phy->radio_rev == 8)
1586         b43legacy_radio_write16(dev, 0x0043, 0x001F);
1587     else {
1588         b43legacy_radio_write16(dev, 0x0052, 0x0000);
1589         b43legacy_radio_write16(dev, 0x0043,
1590                     (b43legacy_radio_read16(dev, 0x0043)
1591                     & 0xFFF0) | 0x0009);
1592     }
1593     b43legacy_phy_write(dev, 0x0058, 0x0000);
1594 
1595     for (i = 0; i < 16; i++) {
1596         b43legacy_phy_write(dev, 0x005A, 0x0480);
1597         b43legacy_phy_write(dev, 0x0059, 0xC810);
1598         b43legacy_phy_write(dev, 0x0058, 0x000D);
1599         if (phy->gmode)
1600             b43legacy_phy_write(dev, 0x0812,
1601                         b43legacy_get_812_value(dev,
1602                         LPD(1, 0, 1)));
1603         b43legacy_phy_write(dev, 0x0015, 0xAFB0);
1604         udelay(10);
1605         if (phy->gmode)
1606             b43legacy_phy_write(dev, 0x0812,
1607                         b43legacy_get_812_value(dev,
1608                         LPD(1, 0, 1)));
1609         b43legacy_phy_write(dev, 0x0015, 0xEFB0);
1610         udelay(10);
1611         if (phy->gmode)
1612             b43legacy_phy_write(dev, 0x0812,
1613                         b43legacy_get_812_value(dev,
1614                         LPD(1, 0, 0)));
1615         b43legacy_phy_write(dev, 0x0015, 0xFFF0);
1616         udelay(20);
1617         tmp1 += b43legacy_phy_read(dev, 0x002D);
1618         b43legacy_phy_write(dev, 0x0058, 0x0000);
1619         if (phy->gmode)
1620             b43legacy_phy_write(dev, 0x0812,
1621                         b43legacy_get_812_value(dev,
1622                         LPD(1, 0, 1)));
1623         b43legacy_phy_write(dev, 0x0015, 0xAFB0);
1624     }
1625 
1626     tmp1++;
1627     tmp1 >>= 9;
1628     udelay(10);
1629     b43legacy_phy_write(dev, 0x0058, 0x0000);
1630 
1631     for (i = 0; i < 16; i++) {
1632         b43legacy_radio_write16(dev, 0x0078, (flip_4bit(i) << 1)
1633                     | 0x0020);
1634         backup[13] = b43legacy_radio_read16(dev, 0x0078);
1635         udelay(10);
1636         for (j = 0; j < 16; j++) {
1637             b43legacy_phy_write(dev, 0x005A, 0x0D80);
1638             b43legacy_phy_write(dev, 0x0059, 0xC810);
1639             b43legacy_phy_write(dev, 0x0058, 0x000D);
1640             if (phy->gmode)
1641                 b43legacy_phy_write(dev, 0x0812,
1642                             b43legacy_get_812_value(dev,
1643                             LPD(1, 0, 1)));
1644             b43legacy_phy_write(dev, 0x0015, 0xAFB0);
1645             udelay(10);
1646             if (phy->gmode)
1647                 b43legacy_phy_write(dev, 0x0812,
1648                             b43legacy_get_812_value(dev,
1649                             LPD(1, 0, 1)));
1650             b43legacy_phy_write(dev, 0x0015, 0xEFB0);
1651             udelay(10);
1652             if (phy->gmode)
1653                 b43legacy_phy_write(dev, 0x0812,
1654                             b43legacy_get_812_value(dev,
1655                             LPD(1, 0, 0)));
1656             b43legacy_phy_write(dev, 0x0015, 0xFFF0);
1657             udelay(10);
1658             tmp2 += b43legacy_phy_read(dev, 0x002D);
1659             b43legacy_phy_write(dev, 0x0058, 0x0000);
1660             if (phy->gmode)
1661                 b43legacy_phy_write(dev, 0x0812,
1662                             b43legacy_get_812_value(dev,
1663                             LPD(1, 0, 1)));
1664             b43legacy_phy_write(dev, 0x0015, 0xAFB0);
1665         }
1666         tmp2++;
1667         tmp2 >>= 8;
1668         if (tmp1 < tmp2)
1669             break;
1670     }
1671 
1672     /* Restore the registers */
1673     b43legacy_phy_write(dev, 0x0015, backup[1]);
1674     b43legacy_radio_write16(dev, 0x0051, backup[14]);
1675     b43legacy_radio_write16(dev, 0x0052, backup[15]);
1676     b43legacy_radio_write16(dev, 0x0043, backup[0]);
1677     b43legacy_phy_write(dev, 0x005A, backup[16]);
1678     b43legacy_phy_write(dev, 0x0059, backup[17]);
1679     b43legacy_phy_write(dev, 0x0058, backup[18]);
1680     b43legacy_write16(dev, 0x03E6, backup[11]);
1681     if (phy->analog != 0)
1682         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, backup[12]);
1683     b43legacy_phy_write(dev, 0x0035, backup[10]);
1684     b43legacy_radio_selectchannel(dev, phy->channel, 1);
1685     if (phy->type == B43legacy_PHYTYPE_B) {
1686         b43legacy_phy_write(dev, 0x0030, backup[2]);
1687         b43legacy_write16(dev, 0x03EC, backup[3]);
1688     } else {
1689         if (phy->gmode) {
1690             b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO,
1691                       (b43legacy_read16(dev,
1692                       B43legacy_MMIO_PHY_RADIO) & 0x7FFF));
1693             b43legacy_phy_write(dev, 0x0811, backup[4]);
1694             b43legacy_phy_write(dev, 0x0812, backup[5]);
1695             b43legacy_phy_write(dev, 0x0814, backup[6]);
1696             b43legacy_phy_write(dev, 0x0815, backup[7]);
1697             b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1698                         backup[8]);
1699             b43legacy_phy_write(dev, 0x0802, backup[9]);
1700             if (phy->rev > 1) {
1701                 b43legacy_phy_write(dev, 0x080F, backup[19]);
1702                 b43legacy_phy_write(dev, 0x0810, backup[20]);
1703             }
1704         }
1705     }
1706     if (i >= 15)
1707         ret = backup[13];
1708 
1709     return ret;
1710 }
1711 
1712 static inline
1713 u16 freq_r3A_value(u16 frequency)
1714 {
1715     u16 value;
1716 
1717     if (frequency < 5091)
1718         value = 0x0040;
1719     else if (frequency < 5321)
1720         value = 0x0000;
1721     else if (frequency < 5806)
1722         value = 0x0080;
1723     else
1724         value = 0x0040;
1725 
1726     return value;
1727 }
1728 
1729 int b43legacy_radio_selectchannel(struct b43legacy_wldev *dev,
1730                   u8 channel,
1731                   int synthetic_pu_workaround)
1732 {
1733     struct b43legacy_phy *phy = &dev->phy;
1734 
1735     if (channel == 0xFF) {
1736         switch (phy->type) {
1737         case B43legacy_PHYTYPE_B:
1738         case B43legacy_PHYTYPE_G:
1739             channel = B43legacy_RADIO_DEFAULT_CHANNEL_BG;
1740             break;
1741         default:
1742             B43legacy_WARN_ON(1);
1743         }
1744     }
1745 
1746 /* TODO: Check if channel is valid - return -EINVAL if not */
1747     if (synthetic_pu_workaround)
1748         b43legacy_synth_pu_workaround(dev, channel);
1749 
1750     b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
1751               channel2freq_bg(channel));
1752 
1753     if (channel == 14) {
1754         if (dev->dev->bus->sprom.country_code == 5)   /* JAPAN) */
1755             b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
1756                           B43legacy_UCODEFLAGS_OFFSET,
1757                           b43legacy_shm_read32(dev,
1758                           B43legacy_SHM_SHARED,
1759                           B43legacy_UCODEFLAGS_OFFSET)
1760                           & ~(1 << 7));
1761         else
1762             b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
1763                           B43legacy_UCODEFLAGS_OFFSET,
1764                           b43legacy_shm_read32(dev,
1765                           B43legacy_SHM_SHARED,
1766                           B43legacy_UCODEFLAGS_OFFSET)
1767                           | (1 << 7));
1768         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
1769                   b43legacy_read16(dev,
1770                   B43legacy_MMIO_CHANNEL_EXT) | (1 << 11));
1771     } else
1772         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
1773                   b43legacy_read16(dev,
1774                   B43legacy_MMIO_CHANNEL_EXT) & 0xF7BF);
1775 
1776     phy->channel = channel;
1777     /*XXX: Using the longer of 2 timeouts (8000 vs 2000 usecs). Specs states
1778      *     that 2000 usecs might suffice. */
1779     msleep(8);
1780 
1781     return 0;
1782 }
1783 
1784 void b43legacy_radio_set_txantenna(struct b43legacy_wldev *dev, u32 val)
1785 {
1786     u16 tmp;
1787 
1788     val <<= 8;
1789     tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0022) & 0xFCFF;
1790     b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0022, tmp | val);
1791     tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x03A8) & 0xFCFF;
1792     b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x03A8, tmp | val);
1793     tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0054) & 0xFCFF;
1794     b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0054, tmp | val);
1795 }
1796 
1797 /* http://bcm-specs.sipsolutions.net/TX_Gain_Base_Band */
1798 static u16 b43legacy_get_txgain_base_band(u16 txpower)
1799 {
1800     u16 ret;
1801 
1802     B43legacy_WARN_ON(txpower > 63);
1803 
1804     if (txpower >= 54)
1805         ret = 2;
1806     else if (txpower >= 49)
1807         ret = 4;
1808     else if (txpower >= 44)
1809         ret = 5;
1810     else
1811         ret = 6;
1812 
1813     return ret;
1814 }
1815 
1816 /* http://bcm-specs.sipsolutions.net/TX_Gain_Radio_Frequency_Power_Amplifier */
1817 static u16 b43legacy_get_txgain_freq_power_amp(u16 txpower)
1818 {
1819     u16 ret;
1820 
1821     B43legacy_WARN_ON(txpower > 63);
1822 
1823     if (txpower >= 32)
1824         ret = 0;
1825     else if (txpower >= 25)
1826         ret = 1;
1827     else if (txpower >= 20)
1828         ret = 2;
1829     else if (txpower >= 12)
1830         ret = 3;
1831     else
1832         ret = 4;
1833 
1834     return ret;
1835 }
1836 
1837 /* http://bcm-specs.sipsolutions.net/TX_Gain_Digital_Analog_Converter */
1838 static u16 b43legacy_get_txgain_dac(u16 txpower)
1839 {
1840     u16 ret;
1841 
1842     B43legacy_WARN_ON(txpower > 63);
1843 
1844     if (txpower >= 54)
1845         ret = txpower - 53;
1846     else if (txpower >= 49)
1847         ret = txpower - 42;
1848     else if (txpower >= 44)
1849         ret = txpower - 37;
1850     else if (txpower >= 32)
1851         ret = txpower - 32;
1852     else if (txpower >= 25)
1853         ret = txpower - 20;
1854     else if (txpower >= 20)
1855         ret = txpower - 13;
1856     else if (txpower >= 12)
1857         ret = txpower - 8;
1858     else
1859         ret = txpower;
1860 
1861     return ret;
1862 }
1863 
1864 void b43legacy_radio_set_txpower_a(struct b43legacy_wldev *dev, u16 txpower)
1865 {
1866     struct b43legacy_phy *phy = &dev->phy;
1867     u16 pamp;
1868     u16 base;
1869     u16 dac;
1870     u16 ilt;
1871 
1872     txpower = clamp_val(txpower, 0, 63);
1873 
1874     pamp = b43legacy_get_txgain_freq_power_amp(txpower);
1875     pamp <<= 5;
1876     pamp &= 0x00E0;
1877     b43legacy_phy_write(dev, 0x0019, pamp);
1878 
1879     base = b43legacy_get_txgain_base_band(txpower);
1880     base &= 0x000F;
1881     b43legacy_phy_write(dev, 0x0017, base | 0x0020);
1882 
1883     ilt = b43legacy_ilt_read(dev, 0x3001);
1884     ilt &= 0x0007;
1885 
1886     dac = b43legacy_get_txgain_dac(txpower);
1887     dac <<= 3;
1888     dac |= ilt;
1889 
1890     b43legacy_ilt_write(dev, 0x3001, dac);
1891 
1892     phy->txpwr_offset = txpower;
1893 
1894     /* TODO: FuncPlaceholder (Adjust BB loft cancel) */
1895 }
1896 
1897 void b43legacy_radio_set_txpower_bg(struct b43legacy_wldev *dev,
1898                     u16 baseband_attenuation,
1899                     u16 radio_attenuation,
1900                     u16 txpower)
1901 {
1902     struct b43legacy_phy *phy = &dev->phy;
1903 
1904     if (baseband_attenuation == 0xFFFF)
1905         baseband_attenuation = phy->bbatt;
1906     if (radio_attenuation == 0xFFFF)
1907         radio_attenuation = phy->rfatt;
1908     if (txpower == 0xFFFF)
1909         txpower = phy->txctl1;
1910     phy->bbatt = baseband_attenuation;
1911     phy->rfatt = radio_attenuation;
1912     phy->txctl1 = txpower;
1913 
1914     B43legacy_WARN_ON(baseband_attenuation > 11);
1915     if (phy->radio_rev < 6)
1916         B43legacy_WARN_ON(radio_attenuation > 9);
1917     else
1918         B43legacy_WARN_ON(radio_attenuation > 31);
1919     B43legacy_WARN_ON(txpower > 7);
1920 
1921     b43legacy_phy_set_baseband_attenuation(dev, baseband_attenuation);
1922     b43legacy_radio_write16(dev, 0x0043, radio_attenuation);
1923     b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0064,
1924                   radio_attenuation);
1925     if (phy->radio_ver == 0x2050)
1926         b43legacy_radio_write16(dev, 0x0052,
1927                     (b43legacy_radio_read16(dev, 0x0052)
1928                     & ~0x0070) | ((txpower << 4) & 0x0070));
1929     /* FIXME: The spec is very weird and unclear here. */
1930     if (phy->type == B43legacy_PHYTYPE_G)
1931         b43legacy_phy_lo_adjust(dev, 0);
1932 }
1933 
1934 u16 b43legacy_default_baseband_attenuation(struct b43legacy_wldev *dev)
1935 {
1936     struct b43legacy_phy *phy = &dev->phy;
1937 
1938     if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
1939         return 0;
1940     return 2;
1941 }
1942 
1943 u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
1944 {
1945     struct b43legacy_phy *phy = &dev->phy;
1946     u16 att = 0xFFFF;
1947 
1948     switch (phy->radio_ver) {
1949     case 0x2053:
1950         switch (phy->radio_rev) {
1951         case 1:
1952             att = 6;
1953             break;
1954         }
1955         break;
1956     case 0x2050:
1957         switch (phy->radio_rev) {
1958         case 0:
1959             att = 5;
1960             break;
1961         case 1:
1962             if (phy->type == B43legacy_PHYTYPE_G) {
1963                 if (is_bcm_board_vendor(dev) &&
1964                     dev->dev->bus->boardinfo.type == 0x421 &&
1965                     dev->dev->bus->sprom.board_rev >= 30)
1966                     att = 3;
1967                 else if (is_bcm_board_vendor(dev) &&
1968                      dev->dev->bus->boardinfo.type == 0x416)
1969                     att = 3;
1970                 else
1971                     att = 1;
1972             } else {
1973                 if (is_bcm_board_vendor(dev) &&
1974                     dev->dev->bus->boardinfo.type == 0x421 &&
1975                     dev->dev->bus->sprom.board_rev >= 30)
1976                     att = 7;
1977                 else
1978                     att = 6;
1979             }
1980             break;
1981         case 2:
1982             if (phy->type == B43legacy_PHYTYPE_G) {
1983                 if (is_bcm_board_vendor(dev) &&
1984                     dev->dev->bus->boardinfo.type == 0x421 &&
1985                     dev->dev->bus->sprom.board_rev >= 30)
1986                     att = 3;
1987                 else if (is_bcm_board_vendor(dev) &&
1988                      dev->dev->bus->boardinfo.type ==
1989                      0x416)
1990                     att = 5;
1991                 else if (dev->dev->bus->chip_id == 0x4320)
1992                     att = 4;
1993                 else
1994                     att = 3;
1995             } else
1996                 att = 6;
1997             break;
1998         case 3:
1999             att = 5;
2000             break;
2001         case 4:
2002         case 5:
2003             att = 1;
2004             break;
2005         case 6:
2006         case 7:
2007             att = 5;
2008             break;
2009         case 8:
2010             att = 0x1A;
2011             break;
2012         case 9:
2013         default:
2014             att = 5;
2015         }
2016     }
2017     if (is_bcm_board_vendor(dev) &&
2018         dev->dev->bus->boardinfo.type == 0x421) {
2019         if (dev->dev->bus->sprom.board_rev < 0x43)
2020             att = 2;
2021         else if (dev->dev->bus->sprom.board_rev < 0x51)
2022             att = 3;
2023     }
2024     if (att == 0xFFFF)
2025         att = 5;
2026 
2027     return att;
2028 }
2029 
2030 u16 b43legacy_default_txctl1(struct b43legacy_wldev *dev)
2031 {
2032     struct b43legacy_phy *phy = &dev->phy;
2033 
2034     if (phy->radio_ver != 0x2050)
2035         return 0;
2036     if (phy->radio_rev == 1)
2037         return 3;
2038     if (phy->radio_rev < 6)
2039         return 2;
2040     if (phy->radio_rev == 8)
2041         return 1;
2042     return 0;
2043 }
2044 
2045 void b43legacy_radio_turn_on(struct b43legacy_wldev *dev)
2046 {
2047     struct b43legacy_phy *phy = &dev->phy;
2048     int err;
2049     u8 channel;
2050 
2051     might_sleep();
2052 
2053     if (phy->radio_on)
2054         return;
2055 
2056     switch (phy->type) {
2057     case B43legacy_PHYTYPE_B:
2058     case B43legacy_PHYTYPE_G:
2059         b43legacy_phy_write(dev, 0x0015, 0x8000);
2060         b43legacy_phy_write(dev, 0x0015, 0xCC00);
2061         b43legacy_phy_write(dev, 0x0015,
2062                     (phy->gmode ? 0x00C0 : 0x0000));
2063         if (phy->radio_off_context.valid) {
2064             /* Restore the RFover values. */
2065             b43legacy_phy_write(dev, B43legacy_PHY_RFOVER,
2066                         phy->radio_off_context.rfover);
2067             b43legacy_phy_write(dev, B43legacy_PHY_RFOVERVAL,
2068                         phy->radio_off_context.rfoverval);
2069             phy->radio_off_context.valid = false;
2070         }
2071         channel = phy->channel;
2072         err = b43legacy_radio_selectchannel(dev,
2073                     B43legacy_RADIO_DEFAULT_CHANNEL_BG, 1);
2074         err |= b43legacy_radio_selectchannel(dev, channel, 0);
2075         B43legacy_WARN_ON(err);
2076         break;
2077     default:
2078         B43legacy_BUG_ON(1);
2079     }
2080     phy->radio_on = true;
2081 }
2082 
2083 void b43legacy_radio_turn_off(struct b43legacy_wldev *dev, bool force)
2084 {
2085     struct b43legacy_phy *phy = &dev->phy;
2086 
2087     if (!phy->radio_on && !force)
2088         return;
2089 
2090     if (phy->type == B43legacy_PHYTYPE_G && dev->dev->id.revision >= 5) {
2091         u16 rfover, rfoverval;
2092 
2093         rfover = b43legacy_phy_read(dev, B43legacy_PHY_RFOVER);
2094         rfoverval = b43legacy_phy_read(dev, B43legacy_PHY_RFOVERVAL);
2095         if (!force) {
2096             phy->radio_off_context.rfover = rfover;
2097             phy->radio_off_context.rfoverval = rfoverval;
2098             phy->radio_off_context.valid = true;
2099         }
2100         b43legacy_phy_write(dev, B43legacy_PHY_RFOVER, rfover | 0x008C);
2101         b43legacy_phy_write(dev, B43legacy_PHY_RFOVERVAL,
2102                     rfoverval & 0xFF73);
2103     } else
2104         b43legacy_phy_write(dev, 0x0015, 0xAA00);
2105     phy->radio_on = false;
2106     b43legacydbg(dev->wl, "Radio initialized\n");
2107 }
2108 
2109 void b43legacy_radio_clear_tssi(struct b43legacy_wldev *dev)
2110 {
2111     struct b43legacy_phy *phy = &dev->phy;
2112 
2113     switch (phy->type) {
2114     case B43legacy_PHYTYPE_B:
2115     case B43legacy_PHYTYPE_G:
2116         b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0058,
2117                       0x7F7F);
2118         b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x005a,
2119                       0x7F7F);
2120         b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0070,
2121                       0x7F7F);
2122         b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0072,
2123                       0x7F7F);
2124         break;
2125     }
2126 }
[ 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!