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

   1 /*
   2
   3   Broadcom B43legacy wireless driver
   4
   5   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
   6                      Stefano Brivio <stefano.brivio@polimi.it>
   7                      Michael Buesch <m@bues.ch>
   8                      Danny van Dyk <kugelfang@gentoo.org>
   9                      Andreas Jaggi <andreas.jaggi@waterwave.ch>
  10   Copyright (c) 2007 Larry Finger <Larry.Finger@lwfinger.net>
  11
  12   Some parts of the code in this file are derived from the ipw2200
  13   driver  Copyright(c) 2003 - 2004 Intel Corporation.
  14
  15   This program is free software; you can redistribute it and/or modify
  16   it under the terms of the GNU General Public License as published by
  17   the Free Software Foundation; either version 2 of the License, or
  18   (at your option) any later version.
  19
  20   This program is distributed in the hope that it will be useful,
  21   but WITHOUT ANY WARRANTY; without even the implied warranty of
  22   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
  23   GNU General Public License for more details.
  24
  25   You should have received a copy of the GNU General Public License
  26   along with this program; see the file COPYING.  If not, write to
  27   the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
  28   Boston, MA 02110-1301, USA.
  29
  30 */
  31
  32 #include <linux/delay.h>
  33
  34 #include "b43legacy.h"
  35 #include "main.h"
  36 #include "phy.h"
  37 #include "radio.h"
  38 #include "ilt.h"
  39
  40
  41 /* Table for b43legacy_radio_calibrationvalue() */
  42 static const u16 rcc_table[16] = {
  43         0x0002, 0x0003, 0x0001, 0x000F,
  44         0x0006, 0x0007, 0x0005, 0x000F,
  45         0x000A, 0x000B, 0x0009, 0x000F,
  46         0x000E, 0x000F, 0x000D, 0x000F,
  47 };
  48
  49 /* Reverse the bits of a 4bit value.
  50  * Example:  1101 is flipped 1011
  51  */
  52 static u16 flip_4bit(u16 value)
  53 {
  54         u16 flipped = 0x0000;
  55
  56         B43legacy_BUG_ON(!((value & ~0x000F) == 0x0000));
  57
  58         flipped |= (value & 0x0001) << 3;
  59         flipped |= (value & 0x0002) << 1;
  60         flipped |= (value & 0x0004) >> 1;
  61         flipped |= (value & 0x0008) >> 3;
  62
  63         return flipped;
  64 }
  65
  66 /* Get the freq, as it has to be written to the device. */
  67 static inline
  68 u16 channel2freq_bg(u8 channel)
  69 {
  70         /* Frequencies are given as frequencies_bg[index] + 2.4GHz
  71          * Starting with channel 1
  72          */
  73         static const u16 frequencies_bg[14] = {
  74                 12, 17, 22, 27,
  75                 32, 37, 42, 47,
  76                 52, 57, 62, 67,
  77                 72, 84,
  78         };
  79
  80         if (unlikely(channel < 1 || channel > 14)) {
  81                 printk(KERN_INFO "b43legacy: Channel %d is out of range\n",
  82                                   channel);
  83                 dump_stack();
  84                 return 2412;
  85         }
  86
  87         return frequencies_bg[channel - 1];
  88 }
  89
  90 void b43legacy_radio_lock(struct b43legacy_wldev *dev)
  91 {
  92         u32 status;
  93
  94         status = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
  95         B43legacy_WARN_ON(status & B43legacy_MACCTL_RADIOLOCK);
  96         status |= B43legacy_MACCTL_RADIOLOCK;
  97         b43legacy_write32(dev, B43legacy_MMIO_MACCTL, status);
  98         udelay(10);
  99 }
 100
 101 void b43legacy_radio_unlock(struct b43legacy_wldev *dev)
 102 {
 103         u32 status;
 104
 105         b43legacy_read16(dev, B43legacy_MMIO_PHY_VER); /* dummy read */
 106         status = b43legacy_read32(dev, B43legacy_MMIO_MACCTL);
 107         B43legacy_WARN_ON(!(status & B43legacy_MACCTL_RADIOLOCK));
 108         status &= ~B43legacy_MACCTL_RADIOLOCK;
 109         b43legacy_write32(dev, B43legacy_MMIO_MACCTL, status);
 110 }
 111
 112 u16 b43legacy_radio_read16(struct b43legacy_wldev *dev, u16 offset)
 113 {
 114         struct b43legacy_phy *phy = &dev->phy;
 115
 116         switch (phy->type) {
 117         case B43legacy_PHYTYPE_B:
 118                 if (phy->radio_ver == 0x2053) {
 119                         if (offset < 0x70)
 120                                 offset += 0x80;
 121                         else if (offset < 0x80)
 122                                 offset += 0x70;
 123                 } else if (phy->radio_ver == 0x2050)
 124                         offset |= 0x80;
 125                 else
 126                         B43legacy_WARN_ON(1);
 127                 break;
 128         case B43legacy_PHYTYPE_G:
 129                 offset |= 0x80;
 130                 break;
 131         default:
 132                 B43legacy_BUG_ON(1);
 133         }
 134
 135         b43legacy_write16(dev, B43legacy_MMIO_RADIO_CONTROL, offset);
 136         return b43legacy_read16(dev, B43legacy_MMIO_RADIO_DATA_LOW);
 137 }
 138
 139 void b43legacy_radio_write16(struct b43legacy_wldev *dev, u16 offset, u16 val)
 140 {
 141         b43legacy_write16(dev, B43legacy_MMIO_RADIO_CONTROL, offset);
 142         b43legacy_write16(dev, B43legacy_MMIO_RADIO_DATA_LOW, val);
 143 }
 144
 145 static void b43legacy_set_all_gains(struct b43legacy_wldev *dev,
 146                                   s16 first, s16 second, s16 third)
 147 {
 148         struct b43legacy_phy *phy = &dev->phy;
 149         u16 i;
 150         u16 start = 0x08;
 151         u16 end = 0x18;
 152         u16 offset = 0x0400;
 153         u16 tmp;
 154
 155         if (phy->rev <= 1) {
 156                 offset = 0x5000;
 157                 start = 0x10;
 158                 end = 0x20;
 159         }
 160
 161         for (i = 0; i < 4; i++)
 162                 b43legacy_ilt_write(dev, offset + i, first);
 163
 164         for (i = start; i < end; i++)
 165                 b43legacy_ilt_write(dev, offset + i, second);
 166
 167         if (third != -1) {
 168                 tmp = ((u16)third << 14) | ((u16)third << 6);
 169                 b43legacy_phy_write(dev, 0x04A0,
 170                                     (b43legacy_phy_read(dev, 0x04A0) & 0xBFBF)
 171                                     | tmp);
 172                 b43legacy_phy_write(dev, 0x04A1,
 173                                     (b43legacy_phy_read(dev, 0x04A1) & 0xBFBF)
 174                                     | tmp);
 175                 b43legacy_phy_write(dev, 0x04A2,
 176                                     (b43legacy_phy_read(dev, 0x04A2) & 0xBFBF)
 177                                     | tmp);
 178         }
 179         b43legacy_dummy_transmission(dev);
 180 }
 181
 182 static void b43legacy_set_original_gains(struct b43legacy_wldev *dev)
 183 {
 184         struct b43legacy_phy *phy = &dev->phy;
 185         u16 i;
 186         u16 tmp;
 187         u16 offset = 0x0400;
 188         u16 start = 0x0008;
 189         u16 end = 0x0018;
 190
 191         if (phy->rev <= 1) {
 192                 offset = 0x5000;
 193                 start = 0x0010;
 194                 end = 0x0020;
 195         }
 196
 197         for (i = 0; i < 4; i++) {
 198                 tmp = (i & 0xFFFC);
 199                 tmp |= (i & 0x0001) << 1;
 200                 tmp |= (i & 0x0002) >> 1;
 201
 202                 b43legacy_ilt_write(dev, offset + i, tmp);
 203         }
 204
 205         for (i = start; i < end; i++)
 206                 b43legacy_ilt_write(dev, offset + i, i - start);
 207
 208         b43legacy_phy_write(dev, 0x04A0,
 209                             (b43legacy_phy_read(dev, 0x04A0) & 0xBFBF)
 210                             | 0x4040);
 211         b43legacy_phy_write(dev, 0x04A1,
 212                             (b43legacy_phy_read(dev, 0x04A1) & 0xBFBF)
 213                             | 0x4040);
 214         b43legacy_phy_write(dev, 0x04A2,
 215                             (b43legacy_phy_read(dev, 0x04A2) & 0xBFBF)
 216                             | 0x4000);
 217         b43legacy_dummy_transmission(dev);
 218 }
 219
 220 /* Synthetic PU workaround */
 221 static void b43legacy_synth_pu_workaround(struct b43legacy_wldev *dev,
 222                                           u8 channel)
 223 {
 224         struct b43legacy_phy *phy = &dev->phy;
 225
 226         might_sleep();
 227
 228         if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6)
 229                 /* We do not need the workaround. */
 230                 return;
 231
 232         if (channel <= 10)
 233                 b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
 234                                   channel2freq_bg(channel + 4));
 235         else
 236                 b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
 237                                   channel2freq_bg(channel));
 238         msleep(1);
 239         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
 240                           channel2freq_bg(channel));
 241 }
 242
 243 u8 b43legacy_radio_aci_detect(struct b43legacy_wldev *dev, u8 channel)
 244 {
 245         struct b43legacy_phy *phy = &dev->phy;
 246         u8 ret = 0;
 247         u16 saved;
 248         u16 rssi;
 249         u16 temp;
 250         int i;
 251         int j = 0;
 252
 253         saved = b43legacy_phy_read(dev, 0x0403);
 254         b43legacy_radio_selectchannel(dev, channel, 0);
 255         b43legacy_phy_write(dev, 0x0403, (saved & 0xFFF8) | 5);
 256         if (phy->aci_hw_rssi)
 257                 rssi = b43legacy_phy_read(dev, 0x048A) & 0x3F;
 258         else
 259                 rssi = saved & 0x3F;
 260         /* clamp temp to signed 5bit */
 261         if (rssi > 32)
 262                 rssi -= 64;
 263         for (i = 0; i < 100; i++) {
 264                 temp = (b43legacy_phy_read(dev, 0x047F) >> 8) & 0x3F;
 265                 if (temp > 32)
 266                         temp -= 64;
 267                 if (temp < rssi)
 268                         j++;
 269                 if (j >= 20)
 270                         ret = 1;
 271         }
 272         b43legacy_phy_write(dev, 0x0403, saved);
 273
 274         return ret;
 275 }
 276
 277 u8 b43legacy_radio_aci_scan(struct b43legacy_wldev *dev)
 278 {
 279         struct b43legacy_phy *phy = &dev->phy;
 280         u8 ret[13] = { 0 };
 281         unsigned int channel = phy->channel;
 282         unsigned int i;
 283         unsigned int j;
 284         unsigned int start;
 285         unsigned int end;
 286
 287         if (!((phy->type == B43legacy_PHYTYPE_G) && (phy->rev > 0)))
 288                 return 0;
 289
 290         b43legacy_phy_lock(dev);
 291         b43legacy_radio_lock(dev);
 292         b43legacy_phy_write(dev, 0x0802,
 293                             b43legacy_phy_read(dev, 0x0802) & 0xFFFC);
 294         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
 295                             b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
 296                             & 0x7FFF);
 297         b43legacy_set_all_gains(dev, 3, 8, 1);
 298
 299         start = (channel - 5 > 0) ? channel - 5 : 1;
 300         end = (channel + 5 < 14) ? channel + 5 : 13;
 301
 302         for (i = start; i <= end; i++) {
 303                 if (abs(channel - i) > 2)
 304                         ret[i-1] = b43legacy_radio_aci_detect(dev, i);
 305         }
 306         b43legacy_radio_selectchannel(dev, channel, 0);
 307         b43legacy_phy_write(dev, 0x0802,
 308                             (b43legacy_phy_read(dev, 0x0802) & 0xFFFC)
 309                             | 0x0003);
 310         b43legacy_phy_write(dev, 0x0403,
 311                             b43legacy_phy_read(dev, 0x0403) & 0xFFF8);
 312         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
 313                             b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
 314                             | 0x8000);
 315         b43legacy_set_original_gains(dev);
 316         for (i = 0; i < 13; i++) {
 317                 if (!ret[i])
 318                         continue;
 319                 end = (i + 5 < 13) ? i + 5 : 13;
 320                 for (j = i; j < end; j++)
 321                         ret[j] = 1;
 322         }
 323         b43legacy_radio_unlock(dev);
 324         b43legacy_phy_unlock(dev);
 325
 326         return ret[channel - 1];
 327 }
 328
 329 /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
 330 void b43legacy_nrssi_hw_write(struct b43legacy_wldev *dev, u16 offset, s16 val)
 331 {
 332         b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_CTRL, offset);
 333         b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_DATA, (u16)val);
 334 }
 335
 336 /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
 337 s16 b43legacy_nrssi_hw_read(struct b43legacy_wldev *dev, u16 offset)
 338 {
 339         u16 val;
 340
 341         b43legacy_phy_write(dev, B43legacy_PHY_NRSSILT_CTRL, offset);
 342         val = b43legacy_phy_read(dev, B43legacy_PHY_NRSSILT_DATA);
 343
 344         return (s16)val;
 345 }
 346
 347 /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
 348 void b43legacy_nrssi_hw_update(struct b43legacy_wldev *dev, u16 val)
 349 {
 350         u16 i;
 351         s16 tmp;
 352
 353         for (i = 0; i < 64; i++) {
 354                 tmp = b43legacy_nrssi_hw_read(dev, i);
 355                 tmp -= val;
 356                 tmp = clamp_val(tmp, -32, 31);
 357                 b43legacy_nrssi_hw_write(dev, i, tmp);
 358         }
 359 }
 360
 361 /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
 362 void b43legacy_nrssi_mem_update(struct b43legacy_wldev *dev)
 363 {
 364         struct b43legacy_phy *phy = &dev->phy;
 365         s16 i;
 366         s16 delta;
 367         s32 tmp;
 368
 369         delta = 0x1F - phy->nrssi[0];
 370         for (i = 0; i < 64; i++) {
 371                 tmp = (i - delta) * phy->nrssislope;
 372                 tmp /= 0x10000;
 373                 tmp += 0x3A;
 374                 tmp = clamp_val(tmp, 0, 0x3F);
 375                 phy->nrssi_lt[i] = tmp;
 376         }
 377 }
 378
 379 static void b43legacy_calc_nrssi_offset(struct b43legacy_wldev *dev)
 380 {
 381         struct b43legacy_phy *phy = &dev->phy;
 382         u16 backup[20] = { 0 };
 383         s16 v47F;
 384         u16 i;
 385         u16 saved = 0xFFFF;
 386
 387         backup[0] = b43legacy_phy_read(dev, 0x0001);
 388         backup[1] = b43legacy_phy_read(dev, 0x0811);
 389         backup[2] = b43legacy_phy_read(dev, 0x0812);
 390         backup[3] = b43legacy_phy_read(dev, 0x0814);
 391         backup[4] = b43legacy_phy_read(dev, 0x0815);
 392         backup[5] = b43legacy_phy_read(dev, 0x005A);
 393         backup[6] = b43legacy_phy_read(dev, 0x0059);
 394         backup[7] = b43legacy_phy_read(dev, 0x0058);
 395         backup[8] = b43legacy_phy_read(dev, 0x000A);
 396         backup[9] = b43legacy_phy_read(dev, 0x0003);
 397         backup[10] = b43legacy_radio_read16(dev, 0x007A);
 398         backup[11] = b43legacy_radio_read16(dev, 0x0043);
 399
 400         b43legacy_phy_write(dev, 0x0429,
 401                             b43legacy_phy_read(dev, 0x0429) & 0x7FFF);
 402         b43legacy_phy_write(dev, 0x0001,
 403                             (b43legacy_phy_read(dev, 0x0001) & 0x3FFF)
 404                             | 0x4000);
 405         b43legacy_phy_write(dev, 0x0811,
 406                             b43legacy_phy_read(dev, 0x0811) | 0x000C);
 407         b43legacy_phy_write(dev, 0x0812,
 408                             (b43legacy_phy_read(dev, 0x0812) & 0xFFF3)
 409                             | 0x0004);
 410         b43legacy_phy_write(dev, 0x0802,
 411                             b43legacy_phy_read(dev, 0x0802) & ~(0x1 | 0x2));
 412         if (phy->rev >= 6) {
 413                 backup[12] = b43legacy_phy_read(dev, 0x002E);
 414                 backup[13] = b43legacy_phy_read(dev, 0x002F);
 415                 backup[14] = b43legacy_phy_read(dev, 0x080F);
 416                 backup[15] = b43legacy_phy_read(dev, 0x0810);
 417                 backup[16] = b43legacy_phy_read(dev, 0x0801);
 418                 backup[17] = b43legacy_phy_read(dev, 0x0060);
 419                 backup[18] = b43legacy_phy_read(dev, 0x0014);
 420                 backup[19] = b43legacy_phy_read(dev, 0x0478);
 421
 422                 b43legacy_phy_write(dev, 0x002E, 0);
 423                 b43legacy_phy_write(dev, 0x002F, 0);
 424                 b43legacy_phy_write(dev, 0x080F, 0);
 425                 b43legacy_phy_write(dev, 0x0810, 0);
 426                 b43legacy_phy_write(dev, 0x0478,
 427                                     b43legacy_phy_read(dev, 0x0478) | 0x0100);
 428                 b43legacy_phy_write(dev, 0x0801,
 429                                     b43legacy_phy_read(dev, 0x0801) | 0x0040);
 430                 b43legacy_phy_write(dev, 0x0060,
 431                                     b43legacy_phy_read(dev, 0x0060) | 0x0040);
 432                 b43legacy_phy_write(dev, 0x0014,
 433                                     b43legacy_phy_read(dev, 0x0014) | 0x0200);
 434         }
 435         b43legacy_radio_write16(dev, 0x007A,
 436                                 b43legacy_radio_read16(dev, 0x007A) | 0x0070);
 437         b43legacy_radio_write16(dev, 0x007A,
 438                                 b43legacy_radio_read16(dev, 0x007A) | 0x0080);
 439         udelay(30);
 440
 441         v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
 442         if (v47F >= 0x20)
 443                 v47F -= 0x40;
 444         if (v47F == 31) {
 445                 for (i = 7; i >= 4; i--) {
 446                         b43legacy_radio_write16(dev, 0x007B, i);
 447                         udelay(20);
 448                         v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8)
 449                                                          & 0x003F);
 450                         if (v47F >= 0x20)
 451                                 v47F -= 0x40;
 452                         if (v47F < 31 && saved == 0xFFFF)
 453                                 saved = i;
 454                 }
 455                 if (saved == 0xFFFF)
 456                         saved = 4;
 457         } else {
 458                 b43legacy_radio_write16(dev, 0x007A,
 459                                         b43legacy_radio_read16(dev, 0x007A)
 460                                         & 0x007F);
 461                 b43legacy_phy_write(dev, 0x0814,
 462                                     b43legacy_phy_read(dev, 0x0814) | 0x0001);
 463                 b43legacy_phy_write(dev, 0x0815,
 464                                     b43legacy_phy_read(dev, 0x0815) & 0xFFFE);
 465                 b43legacy_phy_write(dev, 0x0811,
 466                                     b43legacy_phy_read(dev, 0x0811) | 0x000C);
 467                 b43legacy_phy_write(dev, 0x0812,
 468                                     b43legacy_phy_read(dev, 0x0812) | 0x000C);
 469                 b43legacy_phy_write(dev, 0x0811,
 470                                     b43legacy_phy_read(dev, 0x0811) | 0x0030);
 471                 b43legacy_phy_write(dev, 0x0812,
 472                                     b43legacy_phy_read(dev, 0x0812) | 0x0030);
 473                 b43legacy_phy_write(dev, 0x005A, 0x0480);
 474                 b43legacy_phy_write(dev, 0x0059, 0x0810);
 475                 b43legacy_phy_write(dev, 0x0058, 0x000D);
 476                 if (phy->analog == 0)
 477                         b43legacy_phy_write(dev, 0x0003, 0x0122);
 478                 else
 479                         b43legacy_phy_write(dev, 0x000A,
 480                                             b43legacy_phy_read(dev, 0x000A)
 481                                             | 0x2000);
 482                 b43legacy_phy_write(dev, 0x0814,
 483                                     b43legacy_phy_read(dev, 0x0814) | 0x0004);
 484                 b43legacy_phy_write(dev, 0x0815,
 485                                     b43legacy_phy_read(dev, 0x0815) & 0xFFFB);
 486                 b43legacy_phy_write(dev, 0x0003,
 487                                     (b43legacy_phy_read(dev, 0x0003) & 0xFF9F)
 488                                     | 0x0040);
 489                 b43legacy_radio_write16(dev, 0x007A,
 490                                         b43legacy_radio_read16(dev, 0x007A)
 491                                         | 0x000F);
 492                 b43legacy_set_all_gains(dev, 3, 0, 1);
 493                 b43legacy_radio_write16(dev, 0x0043,
 494                                         (b43legacy_radio_read16(dev, 0x0043)
 495                                         & 0x00F0) | 0x000F);
 496                 udelay(30);
 497                 v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
 498                 if (v47F >= 0x20)
 499                         v47F -= 0x40;
 500                 if (v47F == -32) {
 501                         for (i = 0; i < 4; i++) {
 502                                 b43legacy_radio_write16(dev, 0x007B, i);
 503                                 udelay(20);
 504                                 v47F = (s16)((b43legacy_phy_read(dev, 0x047F) >>
 505                                                                  8) & 0x003F);
 506                                 if (v47F >= 0x20)
 507                                         v47F -= 0x40;
 508                                 if (v47F > -31 && saved == 0xFFFF)
 509                                         saved = i;
 510                         }
 511                         if (saved == 0xFFFF)
 512                                 saved = 3;
 513                 } else
 514                         saved = 0;
 515         }
 516         b43legacy_radio_write16(dev, 0x007B, saved);
 517
 518         if (phy->rev >= 6) {
 519                 b43legacy_phy_write(dev, 0x002E, backup[12]);
 520                 b43legacy_phy_write(dev, 0x002F, backup[13]);
 521                 b43legacy_phy_write(dev, 0x080F, backup[14]);
 522                 b43legacy_phy_write(dev, 0x0810, backup[15]);
 523         }
 524         b43legacy_phy_write(dev, 0x0814, backup[3]);
 525         b43legacy_phy_write(dev, 0x0815, backup[4]);
 526         b43legacy_phy_write(dev, 0x005A, backup[5]);
 527         b43legacy_phy_write(dev, 0x0059, backup[6]);
 528         b43legacy_phy_write(dev, 0x0058, backup[7]);
 529         b43legacy_phy_write(dev, 0x000A, backup[8]);
 530         b43legacy_phy_write(dev, 0x0003, backup[9]);
 531         b43legacy_radio_write16(dev, 0x0043, backup[11]);
 532         b43legacy_radio_write16(dev, 0x007A, backup[10]);
 533         b43legacy_phy_write(dev, 0x0802,
 534                             b43legacy_phy_read(dev, 0x0802) | 0x1 | 0x2);
 535         b43legacy_phy_write(dev, 0x0429,
 536                             b43legacy_phy_read(dev, 0x0429) | 0x8000);
 537         b43legacy_set_original_gains(dev);
 538         if (phy->rev >= 6) {
 539                 b43legacy_phy_write(dev, 0x0801, backup[16]);
 540                 b43legacy_phy_write(dev, 0x0060, backup[17]);
 541                 b43legacy_phy_write(dev, 0x0014, backup[18]);
 542                 b43legacy_phy_write(dev, 0x0478, backup[19]);
 543         }
 544         b43legacy_phy_write(dev, 0x0001, backup[0]);
 545         b43legacy_phy_write(dev, 0x0812, backup[2]);
 546         b43legacy_phy_write(dev, 0x0811, backup[1]);
 547 }
 548
 549 void b43legacy_calc_nrssi_slope(struct b43legacy_wldev *dev)
 550 {
 551         struct b43legacy_phy *phy = &dev->phy;
 552         u16 backup[18] = { 0 };
 553         u16 tmp;
 554         s16 nrssi0;
 555         s16 nrssi1;
 556
 557         switch (phy->type) {
 558         case B43legacy_PHYTYPE_B:
 559                 backup[0] = b43legacy_radio_read16(dev, 0x007A);
 560                 backup[1] = b43legacy_radio_read16(dev, 0x0052);
 561                 backup[2] = b43legacy_radio_read16(dev, 0x0043);
 562                 backup[3] = b43legacy_phy_read(dev, 0x0030);
 563                 backup[4] = b43legacy_phy_read(dev, 0x0026);
 564                 backup[5] = b43legacy_phy_read(dev, 0x0015);
 565                 backup[6] = b43legacy_phy_read(dev, 0x002A);
 566                 backup[7] = b43legacy_phy_read(dev, 0x0020);
 567                 backup[8] = b43legacy_phy_read(dev, 0x005A);
 568                 backup[9] = b43legacy_phy_read(dev, 0x0059);
 569                 backup[10] = b43legacy_phy_read(dev, 0x0058);
 570                 backup[11] = b43legacy_read16(dev, 0x03E2);
 571                 backup[12] = b43legacy_read16(dev, 0x03E6);
 572                 backup[13] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
 573
 574                 tmp  = b43legacy_radio_read16(dev, 0x007A);
 575                 tmp &= (phy->rev >= 5) ? 0x007F : 0x000F;
 576                 b43legacy_radio_write16(dev, 0x007A, tmp);
 577                 b43legacy_phy_write(dev, 0x0030, 0x00FF);
 578                 b43legacy_write16(dev, 0x03EC, 0x7F7F);
 579                 b43legacy_phy_write(dev, 0x0026, 0x0000);
 580                 b43legacy_phy_write(dev, 0x0015,
 581                                     b43legacy_phy_read(dev, 0x0015) | 0x0020);
 582                 b43legacy_phy_write(dev, 0x002A, 0x08A3);
 583                 b43legacy_radio_write16(dev, 0x007A,
 584                                         b43legacy_radio_read16(dev, 0x007A)
 585                                         | 0x0080);
 586
 587                 nrssi0 = (s16)b43legacy_phy_read(dev, 0x0027);
 588                 b43legacy_radio_write16(dev, 0x007A,
 589                                         b43legacy_radio_read16(dev, 0x007A)
 590                                         & 0x007F);
 591                 if (phy->analog >= 2)
 592                         b43legacy_write16(dev, 0x03E6, 0x0040);
 593                 else if (phy->analog == 0)
 594                         b43legacy_write16(dev, 0x03E6, 0x0122);
 595                 else
 596                         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
 597                                           b43legacy_read16(dev,
 598                                           B43legacy_MMIO_CHANNEL_EXT) & 0x2000);
 599                 b43legacy_phy_write(dev, 0x0020, 0x3F3F);
 600                 b43legacy_phy_write(dev, 0x0015, 0xF330);
 601                 b43legacy_radio_write16(dev, 0x005A, 0x0060);
 602                 b43legacy_radio_write16(dev, 0x0043,
 603                                         b43legacy_radio_read16(dev, 0x0043)
 604                                         & 0x00F0);
 605                 b43legacy_phy_write(dev, 0x005A, 0x0480);
 606                 b43legacy_phy_write(dev, 0x0059, 0x0810);
 607                 b43legacy_phy_write(dev, 0x0058, 0x000D);
 608                 udelay(20);
 609
 610                 nrssi1 = (s16)b43legacy_phy_read(dev, 0x0027);
 611                 b43legacy_phy_write(dev, 0x0030, backup[3]);
 612                 b43legacy_radio_write16(dev, 0x007A, backup[0]);
 613                 b43legacy_write16(dev, 0x03E2, backup[11]);
 614                 b43legacy_phy_write(dev, 0x0026, backup[4]);
 615                 b43legacy_phy_write(dev, 0x0015, backup[5]);
 616                 b43legacy_phy_write(dev, 0x002A, backup[6]);
 617                 b43legacy_synth_pu_workaround(dev, phy->channel);
 618                 if (phy->analog != 0)
 619                         b43legacy_write16(dev, 0x03F4, backup[13]);
 620
 621                 b43legacy_phy_write(dev, 0x0020, backup[7]);
 622                 b43legacy_phy_write(dev, 0x005A, backup[8]);
 623                 b43legacy_phy_write(dev, 0x0059, backup[9]);
 624                 b43legacy_phy_write(dev, 0x0058, backup[10]);
 625                 b43legacy_radio_write16(dev, 0x0052, backup[1]);
 626                 b43legacy_radio_write16(dev, 0x0043, backup[2]);
 627
 628                 if (nrssi0 == nrssi1)
 629                         phy->nrssislope = 0x00010000;
 630                 else
 631                         phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
 632
 633                 if (nrssi0 <= -4) {
 634                         phy->nrssi[0] = nrssi0;
 635                         phy->nrssi[1] = nrssi1;
 636                 }
 637                 break;
 638         case B43legacy_PHYTYPE_G:
 639                 if (phy->radio_rev >= 9)
 640                         return;
 641                 if (phy->radio_rev == 8)
 642                         b43legacy_calc_nrssi_offset(dev);
 643
 644                 b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
 645                                     b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
 646                                     & 0x7FFF);
 647                 b43legacy_phy_write(dev, 0x0802,
 648                                     b43legacy_phy_read(dev, 0x0802) & 0xFFFC);
 649                 backup[7] = b43legacy_read16(dev, 0x03E2);
 650                 b43legacy_write16(dev, 0x03E2,
 651                                   b43legacy_read16(dev, 0x03E2) | 0x8000);
 652                 backup[0] = b43legacy_radio_read16(dev, 0x007A);
 653                 backup[1] = b43legacy_radio_read16(dev, 0x0052);
 654                 backup[2] = b43legacy_radio_read16(dev, 0x0043);
 655                 backup[3] = b43legacy_phy_read(dev, 0x0015);
 656                 backup[4] = b43legacy_phy_read(dev, 0x005A);
 657                 backup[5] = b43legacy_phy_read(dev, 0x0059);
 658                 backup[6] = b43legacy_phy_read(dev, 0x0058);
 659                 backup[8] = b43legacy_read16(dev, 0x03E6);
 660                 backup[9] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
 661                 if (phy->rev >= 3) {
 662                         backup[10] = b43legacy_phy_read(dev, 0x002E);
 663                         backup[11] = b43legacy_phy_read(dev, 0x002F);
 664                         backup[12] = b43legacy_phy_read(dev, 0x080F);
 665                         backup[13] = b43legacy_phy_read(dev,
 666                                                 B43legacy_PHY_G_LO_CONTROL);
 667                         backup[14] = b43legacy_phy_read(dev, 0x0801);
 668                         backup[15] = b43legacy_phy_read(dev, 0x0060);
 669                         backup[16] = b43legacy_phy_read(dev, 0x0014);
 670                         backup[17] = b43legacy_phy_read(dev, 0x0478);
 671                         b43legacy_phy_write(dev, 0x002E, 0);
 672                         b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL, 0);
 673                         switch (phy->rev) {
 674                         case 4: case 6: case 7:
 675                                 b43legacy_phy_write(dev, 0x0478,
 676                                                     b43legacy_phy_read(dev,
 677                                                     0x0478) | 0x0100);
 678                                 b43legacy_phy_write(dev, 0x0801,
 679                                                     b43legacy_phy_read(dev,
 680                                                     0x0801) | 0x0040);
 681                                 break;
 682                         case 3: case 5:
 683                                 b43legacy_phy_write(dev, 0x0801,
 684                                                     b43legacy_phy_read(dev,
 685                                                     0x0801) & 0xFFBF);
 686                                 break;
 687                         }
 688                         b43legacy_phy_write(dev, 0x0060,
 689                                             b43legacy_phy_read(dev, 0x0060)
 690                                             | 0x0040);
 691                         b43legacy_phy_write(dev, 0x0014,
 692                                             b43legacy_phy_read(dev, 0x0014)
 693                                             | 0x0200);
 694                 }
 695                 b43legacy_radio_write16(dev, 0x007A,
 696                                         b43legacy_radio_read16(dev, 0x007A)
 697                                         | 0x0070);
 698                 b43legacy_set_all_gains(dev, 0, 8, 0);
 699                 b43legacy_radio_write16(dev, 0x007A,
 700                                         b43legacy_radio_read16(dev, 0x007A)
 701                                         & 0x00F7);
 702                 if (phy->rev >= 2) {
 703                         b43legacy_phy_write(dev, 0x0811,
 704                                             (b43legacy_phy_read(dev, 0x0811)
 705                                             & 0xFFCF) | 0x0030);
 706                         b43legacy_phy_write(dev, 0x0812,
 707                                             (b43legacy_phy_read(dev, 0x0812)
 708                                             & 0xFFCF) | 0x0010);
 709                 }
 710                 b43legacy_radio_write16(dev, 0x007A,
 711                                         b43legacy_radio_read16(dev, 0x007A)
 712                                         | 0x0080);
 713                 udelay(20);
 714
 715                 nrssi0 = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
 716                 if (nrssi0 >= 0x0020)
 717                         nrssi0 -= 0x0040;
 718
 719                 b43legacy_radio_write16(dev, 0x007A,
 720                                         b43legacy_radio_read16(dev, 0x007A)
 721                                         & 0x007F);
 722                 if (phy->analog >= 2)
 723                         b43legacy_phy_write(dev, 0x0003,
 724                                             (b43legacy_phy_read(dev, 0x0003)
 725                                             & 0xFF9F) | 0x0040);
 726
 727                 b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
 728                                   b43legacy_read16(dev,
 729                                   B43legacy_MMIO_CHANNEL_EXT) | 0x2000);
 730                 b43legacy_radio_write16(dev, 0x007A,
 731                                         b43legacy_radio_read16(dev, 0x007A)
 732                                         | 0x000F);
 733                 b43legacy_phy_write(dev, 0x0015, 0xF330);
 734                 if (phy->rev >= 2) {
 735                         b43legacy_phy_write(dev, 0x0812,
 736                                             (b43legacy_phy_read(dev, 0x0812)
 737                                             & 0xFFCF) | 0x0020);
 738                         b43legacy_phy_write(dev, 0x0811,
 739                                             (b43legacy_phy_read(dev, 0x0811)
 740                                             & 0xFFCF) | 0x0020);
 741                 }
 742
 743                 b43legacy_set_all_gains(dev, 3, 0, 1);
 744                 if (phy->radio_rev == 8)
 745                         b43legacy_radio_write16(dev, 0x0043, 0x001F);
 746                 else {
 747                         tmp = b43legacy_radio_read16(dev, 0x0052) & 0xFF0F;
 748                         b43legacy_radio_write16(dev, 0x0052, tmp | 0x0060);
 749                         tmp = b43legacy_radio_read16(dev, 0x0043) & 0xFFF0;
 750                         b43legacy_radio_write16(dev, 0x0043, tmp | 0x0009);
 751                 }
 752                 b43legacy_phy_write(dev, 0x005A, 0x0480);
 753                 b43legacy_phy_write(dev, 0x0059, 0x0810);
 754                 b43legacy_phy_write(dev, 0x0058, 0x000D);
 755                 udelay(20);
 756                 nrssi1 = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
 757                 if (nrssi1 >= 0x0020)
 758                         nrssi1 -= 0x0040;
 759                 if (nrssi0 == nrssi1)
 760                         phy->nrssislope = 0x00010000;
 761                 else
 762                         phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
 763                 if (nrssi0 >= -4) {
 764                         phy->nrssi[0] = nrssi1;
 765                         phy->nrssi[1] = nrssi0;
 766                 }
 767                 if (phy->rev >= 3) {
 768                         b43legacy_phy_write(dev, 0x002E, backup[10]);
 769                         b43legacy_phy_write(dev, 0x002F, backup[11]);
 770                         b43legacy_phy_write(dev, 0x080F, backup[12]);
 771                         b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL,
 772                                             backup[13]);
 773                 }
 774                 if (phy->rev >= 2) {
 775                         b43legacy_phy_write(dev, 0x0812,
 776                                             b43legacy_phy_read(dev, 0x0812)
 777                                             & 0xFFCF);
 778                         b43legacy_phy_write(dev, 0x0811,
 779                                             b43legacy_phy_read(dev, 0x0811)
 780                                             & 0xFFCF);
 781                 }
 782
 783                 b43legacy_radio_write16(dev, 0x007A, backup[0]);
 784                 b43legacy_radio_write16(dev, 0x0052, backup[1]);
 785                 b43legacy_radio_write16(dev, 0x0043, backup[2]);
 786                 b43legacy_write16(dev, 0x03E2, backup[7]);
 787                 b43legacy_write16(dev, 0x03E6, backup[8]);
 788                 b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, backup[9]);
 789                 b43legacy_phy_write(dev, 0x0015, backup[3]);
 790                 b43legacy_phy_write(dev, 0x005A, backup[4]);
 791                 b43legacy_phy_write(dev, 0x0059, backup[5]);
 792                 b43legacy_phy_write(dev, 0x0058, backup[6]);
 793                 b43legacy_synth_pu_workaround(dev, phy->channel);
 794                 b43legacy_phy_write(dev, 0x0802,
 795                                     b43legacy_phy_read(dev, 0x0802) | 0x0003);
 796                 b43legacy_set_original_gains(dev);
 797                 b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
 798                                     b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
 799                                     | 0x8000);
 800                 if (phy->rev >= 3) {
 801                         b43legacy_phy_write(dev, 0x0801, backup[14]);
 802                         b43legacy_phy_write(dev, 0x0060, backup[15]);
 803                         b43legacy_phy_write(dev, 0x0014, backup[16]);
 804                         b43legacy_phy_write(dev, 0x0478, backup[17]);
 805                 }
 806                 b43legacy_nrssi_mem_update(dev);
 807                 b43legacy_calc_nrssi_threshold(dev);
 808                 break;
 809         default:
 810                 B43legacy_BUG_ON(1);
 811         }
 812 }
 813
 814 void b43legacy_calc_nrssi_threshold(struct b43legacy_wldev *dev)
 815 {
 816         struct b43legacy_phy *phy = &dev->phy;
 817         s32 threshold;
 818         s32 a;
 819         s32 b;
 820         s16 tmp16;
 821         u16 tmp_u16;
 822
 823         switch (phy->type) {
 824         case B43legacy_PHYTYPE_B: {
 825                 if (phy->radio_ver != 0x2050)
 826                         return;
 827                 if (!(dev->dev->bus->sprom.boardflags_lo &
 828                     B43legacy_BFL_RSSI))
 829                         return;
 830
 831                 if (phy->radio_rev >= 6) {
 832                         threshold = (phy->nrssi[1] - phy->nrssi[0]) * 32;
 833                         threshold += 20 * (phy->nrssi[0] + 1);
 834                         threshold /= 40;
 835                 } else
 836                         threshold = phy->nrssi[1] - 5;
 837
 838                 threshold = clamp_val(threshold, 0, 0x3E);
 839                 b43legacy_phy_read(dev, 0x0020); /* dummy read */
 840                 b43legacy_phy_write(dev, 0x0020, (((u16)threshold) << 8)
 841                                     | 0x001C);
 842
 843                 if (phy->radio_rev >= 6) {
 844                         b43legacy_phy_write(dev, 0x0087, 0x0E0D);
 845                         b43legacy_phy_write(dev, 0x0086, 0x0C0B);
 846                         b43legacy_phy_write(dev, 0x0085, 0x0A09);
 847                         b43legacy_phy_write(dev, 0x0084, 0x0808);
 848                         b43legacy_phy_write(dev, 0x0083, 0x0808);
 849                         b43legacy_phy_write(dev, 0x0082, 0x0604);
 850                         b43legacy_phy_write(dev, 0x0081, 0x0302);
 851                         b43legacy_phy_write(dev, 0x0080, 0x0100);
 852                 }
 853                 break;
 854         }
 855         case B43legacy_PHYTYPE_G:
 856                 if (!phy->gmode ||
 857                     !(dev->dev->bus->sprom.boardflags_lo &
 858                     B43legacy_BFL_RSSI)) {
 859                         tmp16 = b43legacy_nrssi_hw_read(dev, 0x20);
 860                         if (tmp16 >= 0x20)
 861                                 tmp16 -= 0x40;
 862                         if (tmp16 < 3)
 863                                 b43legacy_phy_write(dev, 0x048A,
 864                                                     (b43legacy_phy_read(dev,
 865                                                     0x048A) & 0xF000) | 0x09EB);
 866                         else
 867                                 b43legacy_phy_write(dev, 0x048A,
 868                                                     (b43legacy_phy_read(dev,
 869                                                     0x048A) & 0xF000) | 0x0AED);
 870                 } else {
 871                         if (phy->interfmode ==
 872                             B43legacy_RADIO_INTERFMODE_NONWLAN) {
 873                                 a = 0xE;
 874                                 b = 0xA;
 875                         } else if (!phy->aci_wlan_automatic &&
 876                                     phy->aci_enable) {
 877                                 a = 0x13;
 878                                 b = 0x12;
 879                         } else {
 880                                 a = 0xE;
 881                                 b = 0x11;
 882                         }
 883
 884                         a = a * (phy->nrssi[1] - phy->nrssi[0]);
 885                         a += (phy->nrssi[0] << 6);
 886                         if (a < 32)
 887                                 a += 31;
 888                         else
 889                                 a += 32;
 890                         a = a >> 6;
 891                         a = clamp_val(a, -31, 31);
 892
 893                         b = b * (phy->nrssi[1] - phy->nrssi[0]);
 894                         b += (phy->nrssi[0] << 6);
 895                         if (b < 32)
 896                                 b += 31;
 897                         else
 898                                 b += 32;
 899                         b = b >> 6;
 900                         b = clamp_val(b, -31, 31);
 901
 902                         tmp_u16 = b43legacy_phy_read(dev, 0x048A) & 0xF000;
 903                         tmp_u16 |= ((u32)b & 0x0000003F);
 904                         tmp_u16 |= (((u32)a & 0x0000003F) << 6);
 905                         b43legacy_phy_write(dev, 0x048A, tmp_u16);
 906                 }
 907                 break;
 908         default:
 909                 B43legacy_BUG_ON(1);
 910         }
 911 }
 912
 913 /* Stack implementation to save/restore values from the
 914  * interference mitigation code.
 915  * It is save to restore values in random order.
 916  */
 917 static void _stack_save(u32 *_stackptr, size_t *stackidx,
 918                         u8 id, u16 offset, u16 value)
 919 {
 920         u32 *stackptr = &(_stackptr[*stackidx]);
 921
 922         B43legacy_WARN_ON(!((offset & 0xE000) == 0x0000));
 923         B43legacy_WARN_ON(!((id & 0xF8) == 0x00));
 924         *stackptr = offset;
 925         *stackptr |= ((u32)id) << 13;
 926         *stackptr |= ((u32)value) << 16;
 927         (*stackidx)++;
 928         B43legacy_WARN_ON(!(*stackidx < B43legacy_INTERFSTACK_SIZE));
 929 }
 930
 931 static u16 _stack_restore(u32 *stackptr,
 932                           u8 id, u16 offset)
 933 {
 934         size_t i;
 935
 936         B43legacy_WARN_ON(!((offset & 0xE000) == 0x0000));
 937         B43legacy_WARN_ON(!((id & 0xF8) == 0x00));
 938         for (i = 0; i < B43legacy_INTERFSTACK_SIZE; i++, stackptr++) {
 939                 if ((*stackptr & 0x00001FFF) != offset)
 940                         continue;
 941                 if (((*stackptr & 0x00007000) >> 13) != id)
 942                         continue;
 943                 return ((*stackptr & 0xFFFF0000) >> 16);
 944         }
 945         B43legacy_BUG_ON(1);
 946
 947         return 0;
 948 }
 949
 950 #define phy_stacksave(offset)                                   \
 951         do {                                                    \
 952                 _stack_save(stack, &stackidx, 0x1, (offset),    \
 953                             b43legacy_phy_read(dev, (offset))); \
 954         } while (0)
 955 #define phy_stackrestore(offset)                                \
 956         do {                                                    \
 957                 b43legacy_phy_write(dev, (offset),              \
 958                                     _stack_restore(stack, 0x1,  \
 959                                     (offset)));                 \
 960         } while (0)
 961 #define radio_stacksave(offset)                                         \
 962         do {                                                            \
 963                 _stack_save(stack, &stackidx, 0x2, (offset),            \
 964                             b43legacy_radio_read16(dev, (offset)));     \
 965         } while (0)
 966 #define radio_stackrestore(offset)                                      \
 967         do {                                                            \
 968                 b43legacy_radio_write16(dev, (offset),                  \
 969                                         _stack_restore(stack, 0x2,      \
 970                                         (offset)));                     \
 971         } while (0)
 972 #define ilt_stacksave(offset)                                   \
 973         do {                                                    \
 974                 _stack_save(stack, &stackidx, 0x3, (offset),    \
 975                             b43legacy_ilt_read(dev, (offset))); \
 976         } while (0)
 977 #define ilt_stackrestore(offset)                                \
 978         do {                                                    \
 979                 b43legacy_ilt_write(dev, (offset),              \
 980                                   _stack_restore(stack, 0x3,    \
 981                                                  (offset)));    \
 982         } while (0)
 983
 984 static void
 985 b43legacy_radio_interference_mitigation_enable(struct b43legacy_wldev *dev,
 986                                                int mode)
 987 {
 988         struct b43legacy_phy *phy = &dev->phy;
 989         u16 tmp;
 990         u16 flipped;
 991         u32 tmp32;
 992         size_t stackidx = 0;
 993         u32 *stack = phy->interfstack;
 994
 995         switch (mode) {
 996         case B43legacy_RADIO_INTERFMODE_NONWLAN:
 997                 if (phy->rev != 1) {
 998                         b43legacy_phy_write(dev, 0x042B,
 999                                             b43legacy_phy_read(dev, 0x042B)
1000                                             | 0x0800);
1001                         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1002                                             b43legacy_phy_read(dev,
1003                                             B43legacy_PHY_G_CRS) & ~0x4000);
1004                         break;
1005                 }
1006                 radio_stacksave(0x0078);
1007                 tmp = (b43legacy_radio_read16(dev, 0x0078) & 0x001E);
1008                 flipped = flip_4bit(tmp);
1009                 if (flipped < 10 && flipped >= 8)
1010                         flipped = 7;
1011                 else if (flipped >= 10)
1012                         flipped -= 3;
1013                 flipped = flip_4bit(flipped);
1014                 flipped = (flipped << 1) | 0x0020;
1015                 b43legacy_radio_write16(dev, 0x0078, flipped);
1016
1017                 b43legacy_calc_nrssi_threshold(dev);
1018
1019                 phy_stacksave(0x0406);
1020                 b43legacy_phy_write(dev, 0x0406, 0x7E28);
1021
1022                 b43legacy_phy_write(dev, 0x042B,
1023                                     b43legacy_phy_read(dev, 0x042B) | 0x0800);
1024                 b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
1025                                     b43legacy_phy_read(dev,
1026                                     B43legacy_PHY_RADIO_BITFIELD) | 0x1000);
1027
1028                 phy_stacksave(0x04A0);
1029                 b43legacy_phy_write(dev, 0x04A0,
1030                                     (b43legacy_phy_read(dev, 0x04A0) & 0xC0C0)
1031                                     | 0x0008);
1032                 phy_stacksave(0x04A1);
1033                 b43legacy_phy_write(dev, 0x04A1,
1034                                     (b43legacy_phy_read(dev, 0x04A1) & 0xC0C0)
1035                                     | 0x0605);
1036                 phy_stacksave(0x04A2);
1037                 b43legacy_phy_write(dev, 0x04A2,
1038                                     (b43legacy_phy_read(dev, 0x04A2) & 0xC0C0)
1039                                     | 0x0204);
1040                 phy_stacksave(0x04A8);
1041                 b43legacy_phy_write(dev, 0x04A8,
1042                                     (b43legacy_phy_read(dev, 0x04A8) & 0xC0C0)
1043                                     | 0x0803);
1044                 phy_stacksave(0x04AB);
1045                 b43legacy_phy_write(dev, 0x04AB,
1046                                     (b43legacy_phy_read(dev, 0x04AB) & 0xC0C0)
1047                                     | 0x0605);
1048
1049                 phy_stacksave(0x04A7);
1050                 b43legacy_phy_write(dev, 0x04A7, 0x0002);
1051                 phy_stacksave(0x04A3);
1052                 b43legacy_phy_write(dev, 0x04A3, 0x287A);
1053                 phy_stacksave(0x04A9);
1054                 b43legacy_phy_write(dev, 0x04A9, 0x2027);
1055                 phy_stacksave(0x0493);
1056                 b43legacy_phy_write(dev, 0x0493, 0x32F5);
1057                 phy_stacksave(0x04AA);
1058                 b43legacy_phy_write(dev, 0x04AA, 0x2027);
1059                 phy_stacksave(0x04AC);
1060                 b43legacy_phy_write(dev, 0x04AC, 0x32F5);
1061                 break;
1062         case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
1063                 if (b43legacy_phy_read(dev, 0x0033) & 0x0800)
1064                         break;
1065
1066                 phy->aci_enable = true;
1067
1068                 phy_stacksave(B43legacy_PHY_RADIO_BITFIELD);
1069                 phy_stacksave(B43legacy_PHY_G_CRS);
1070                 if (phy->rev < 2)
1071                         phy_stacksave(0x0406);
1072                 else {
1073                         phy_stacksave(0x04C0);
1074                         phy_stacksave(0x04C1);
1075                 }
1076                 phy_stacksave(0x0033);
1077                 phy_stacksave(0x04A7);
1078                 phy_stacksave(0x04A3);
1079                 phy_stacksave(0x04A9);
1080                 phy_stacksave(0x04AA);
1081                 phy_stacksave(0x04AC);
1082                 phy_stacksave(0x0493);
1083                 phy_stacksave(0x04A1);
1084                 phy_stacksave(0x04A0);
1085                 phy_stacksave(0x04A2);
1086                 phy_stacksave(0x048A);
1087                 phy_stacksave(0x04A8);
1088                 phy_stacksave(0x04AB);
1089                 if (phy->rev == 2) {
1090                         phy_stacksave(0x04AD);
1091                         phy_stacksave(0x04AE);
1092                 } else if (phy->rev >= 3) {
1093                         phy_stacksave(0x04AD);
1094                         phy_stacksave(0x0415);
1095                         phy_stacksave(0x0416);
1096                         phy_stacksave(0x0417);
1097                         ilt_stacksave(0x1A00 + 0x2);
1098                         ilt_stacksave(0x1A00 + 0x3);
1099                 }
1100                 phy_stacksave(0x042B);
1101                 phy_stacksave(0x048C);
1102
1103                 b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
1104                                     b43legacy_phy_read(dev,
1105                                     B43legacy_PHY_RADIO_BITFIELD) & ~0x1000);
1106                 b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1107                                     (b43legacy_phy_read(dev,
1108                                     B43legacy_PHY_G_CRS)
1109                                     & 0xFFFC) | 0x0002);
1110
1111                 b43legacy_phy_write(dev, 0x0033, 0x0800);
1112                 b43legacy_phy_write(dev, 0x04A3, 0x2027);
1113                 b43legacy_phy_write(dev, 0x04A9, 0x1CA8);
1114                 b43legacy_phy_write(dev, 0x0493, 0x287A);
1115                 b43legacy_phy_write(dev, 0x04AA, 0x1CA8);
1116                 b43legacy_phy_write(dev, 0x04AC, 0x287A);
1117
1118                 b43legacy_phy_write(dev, 0x04A0,
1119                                     (b43legacy_phy_read(dev, 0x04A0)
1120                                     & 0xFFC0) | 0x001A);
1121                 b43legacy_phy_write(dev, 0x04A7, 0x000D);
1122
1123                 if (phy->rev < 2)
1124                         b43legacy_phy_write(dev, 0x0406, 0xFF0D);
1125                 else if (phy->rev == 2) {
1126                         b43legacy_phy_write(dev, 0x04C0, 0xFFFF);
1127                         b43legacy_phy_write(dev, 0x04C1, 0x00A9);
1128                 } else {
1129                         b43legacy_phy_write(dev, 0x04C0, 0x00C1);
1130                         b43legacy_phy_write(dev, 0x04C1, 0x0059);
1131                 }
1132
1133                 b43legacy_phy_write(dev, 0x04A1,
1134                                     (b43legacy_phy_read(dev, 0x04A1)
1135                                     & 0xC0FF) | 0x1800);
1136                 b43legacy_phy_write(dev, 0x04A1,
1137                                     (b43legacy_phy_read(dev, 0x04A1)
1138                                     & 0xFFC0) | 0x0015);
1139                 b43legacy_phy_write(dev, 0x04A8,
1140                                     (b43legacy_phy_read(dev, 0x04A8)
1141                                     & 0xCFFF) | 0x1000);
1142                 b43legacy_phy_write(dev, 0x04A8,
1143                                     (b43legacy_phy_read(dev, 0x04A8)
1144                                     & 0xF0FF) | 0x0A00);
1145                 b43legacy_phy_write(dev, 0x04AB,
1146                                     (b43legacy_phy_read(dev, 0x04AB)
1147                                     & 0xCFFF) | 0x1000);
1148                 b43legacy_phy_write(dev, 0x04AB,
1149                                     (b43legacy_phy_read(dev, 0x04AB)
1150                                     & 0xF0FF) | 0x0800);
1151                 b43legacy_phy_write(dev, 0x04AB,
1152                                     (b43legacy_phy_read(dev, 0x04AB)
1153                                     & 0xFFCF) | 0x0010);
1154                 b43legacy_phy_write(dev, 0x04AB,
1155                                     (b43legacy_phy_read(dev, 0x04AB)
1156                                     & 0xFFF0) | 0x0005);
1157                 b43legacy_phy_write(dev, 0x04A8,
1158                                     (b43legacy_phy_read(dev, 0x04A8)
1159                                     & 0xFFCF) | 0x0010);
1160                 b43legacy_phy_write(dev, 0x04A8,
1161                                     (b43legacy_phy_read(dev, 0x04A8)
1162                                     & 0xFFF0) | 0x0006);
1163                 b43legacy_phy_write(dev, 0x04A2,
1164                                     (b43legacy_phy_read(dev, 0x04A2)
1165                                     & 0xF0FF) | 0x0800);
1166                 b43legacy_phy_write(dev, 0x04A0,
1167                                     (b43legacy_phy_read(dev, 0x04A0)
1168                                     & 0xF0FF) | 0x0500);
1169                 b43legacy_phy_write(dev, 0x04A2,
1170                                     (b43legacy_phy_read(dev, 0x04A2)
1171                                     & 0xFFF0) | 0x000B);
1172
1173                 if (phy->rev >= 3) {
1174                         b43legacy_phy_write(dev, 0x048A,
1175                                             b43legacy_phy_read(dev, 0x048A)
1176                                             & ~0x8000);
1177                         b43legacy_phy_write(dev, 0x0415,
1178                                             (b43legacy_phy_read(dev, 0x0415)
1179                                             & 0x8000) | 0x36D8);
1180                         b43legacy_phy_write(dev, 0x0416,
1181                                             (b43legacy_phy_read(dev, 0x0416)
1182                                             & 0x8000) | 0x36D8);
1183                         b43legacy_phy_write(dev, 0x0417,
1184                                             (b43legacy_phy_read(dev, 0x0417)
1185                                             & 0xFE00) | 0x016D);
1186                 } else {
1187                         b43legacy_phy_write(dev, 0x048A,
1188                                             b43legacy_phy_read(dev, 0x048A)
1189                                             | 0x1000);
1190                         b43legacy_phy_write(dev, 0x048A,
1191                                             (b43legacy_phy_read(dev, 0x048A)
1192                                             & 0x9FFF) | 0x2000);
1193                         tmp32 = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
1194                                             B43legacy_UCODEFLAGS_OFFSET);
1195                         if (!(tmp32 & 0x800)) {
1196                                 tmp32 |= 0x800;
1197                                 b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
1198                                             B43legacy_UCODEFLAGS_OFFSET,
1199                                             tmp32);
1200                         }
1201                 }
1202                 if (phy->rev >= 2)
1203                         b43legacy_phy_write(dev, 0x042B,
1204                                             b43legacy_phy_read(dev, 0x042B)
1205                                             | 0x0800);
1206                 b43legacy_phy_write(dev, 0x048C,
1207                                     (b43legacy_phy_read(dev, 0x048C)
1208                                     & 0xF0FF) | 0x0200);
1209                 if (phy->rev == 2) {
1210                         b43legacy_phy_write(dev, 0x04AE,
1211                                             (b43legacy_phy_read(dev, 0x04AE)
1212                                             & 0xFF00) | 0x007F);
1213                         b43legacy_phy_write(dev, 0x04AD,
1214                                             (b43legacy_phy_read(dev, 0x04AD)
1215                                             & 0x00FF) | 0x1300);
1216                 } else if (phy->rev >= 6) {
1217                         b43legacy_ilt_write(dev, 0x1A00 + 0x3, 0x007F);
1218                         b43legacy_ilt_write(dev, 0x1A00 + 0x2, 0x007F);
1219                         b43legacy_phy_write(dev, 0x04AD,
1220                                             b43legacy_phy_read(dev, 0x04AD)
1221                                             & 0x00FF);
1222                 }
1223                 b43legacy_calc_nrssi_slope(dev);
1224                 break;
1225         default:
1226                 B43legacy_BUG_ON(1);
1227         }
1228 }
1229
1230 static void
1231 b43legacy_radio_interference_mitigation_disable(struct b43legacy_wldev *dev,
1232                                                 int mode)
1233 {
1234         struct b43legacy_phy *phy = &dev->phy;
1235         u32 tmp32;
1236         u32 *stack = phy->interfstack;
1237
1238         switch (mode) {
1239         case B43legacy_RADIO_INTERFMODE_NONWLAN:
1240                 if (phy->rev != 1) {
1241                         b43legacy_phy_write(dev, 0x042B,
1242                                             b43legacy_phy_read(dev, 0x042B)
1243                                             & ~0x0800);
1244                         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1245                                             b43legacy_phy_read(dev,
1246                                             B43legacy_PHY_G_CRS) | 0x4000);
1247                         break;
1248                 }
1249                 phy_stackrestore(0x0078);
1250                 b43legacy_calc_nrssi_threshold(dev);
1251                 phy_stackrestore(0x0406);
1252                 b43legacy_phy_write(dev, 0x042B,
1253                                     b43legacy_phy_read(dev, 0x042B) & ~0x0800);
1254                 if (!dev->bad_frames_preempt)
1255                         b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
1256                                             b43legacy_phy_read(dev,
1257                                             B43legacy_PHY_RADIO_BITFIELD)
1258                                             & ~(1 << 11));
1259                 b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1260                                     b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
1261                                     | 0x4000);
1262                 phy_stackrestore(0x04A0);
1263                 phy_stackrestore(0x04A1);
1264                 phy_stackrestore(0x04A2);
1265                 phy_stackrestore(0x04A8);
1266                 phy_stackrestore(0x04AB);
1267                 phy_stackrestore(0x04A7);
1268                 phy_stackrestore(0x04A3);
1269                 phy_stackrestore(0x04A9);
1270                 phy_stackrestore(0x0493);
1271                 phy_stackrestore(0x04AA);
1272                 phy_stackrestore(0x04AC);
1273                 break;
1274         case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
1275                 if (!(b43legacy_phy_read(dev, 0x0033) & 0x0800))
1276                         break;
1277
1278                 phy->aci_enable = false;
1279
1280                 phy_stackrestore(B43legacy_PHY_RADIO_BITFIELD);
1281                 phy_stackrestore(B43legacy_PHY_G_CRS);
1282                 phy_stackrestore(0x0033);
1283                 phy_stackrestore(0x04A3);
1284                 phy_stackrestore(0x04A9);
1285                 phy_stackrestore(0x0493);
1286                 phy_stackrestore(0x04AA);
1287                 phy_stackrestore(0x04AC);
1288                 phy_stackrestore(0x04A0);
1289                 phy_stackrestore(0x04A7);
1290                 if (phy->rev >= 2) {
1291                         phy_stackrestore(0x04C0);
1292                         phy_stackrestore(0x04C1);
1293                 } else
1294                         phy_stackrestore(0x0406);
1295                 phy_stackrestore(0x04A1);
1296                 phy_stackrestore(0x04AB);
1297                 phy_stackrestore(0x04A8);
1298                 if (phy->rev == 2) {
1299                         phy_stackrestore(0x04AD);
1300                         phy_stackrestore(0x04AE);
1301                 } else if (phy->rev >= 3) {
1302                         phy_stackrestore(0x04AD);
1303                         phy_stackrestore(0x0415);
1304                         phy_stackrestore(0x0416);
1305                         phy_stackrestore(0x0417);
1306                         ilt_stackrestore(0x1A00 + 0x2);
1307                         ilt_stackrestore(0x1A00 + 0x3);
1308                 }
1309                 phy_stackrestore(0x04A2);
1310                 phy_stackrestore(0x04A8);
1311                 phy_stackrestore(0x042B);
1312                 phy_stackrestore(0x048C);
1313                 tmp32 = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
1314                                              B43legacy_UCODEFLAGS_OFFSET);
1315                 if (tmp32 & 0x800) {
1316                         tmp32 &= ~0x800;
1317                         b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
1318                                               B43legacy_UCODEFLAGS_OFFSET,
1319                                               tmp32);
1320                 }
1321                 b43legacy_calc_nrssi_slope(dev);
1322                 break;
1323         default:
1324                 B43legacy_BUG_ON(1);
1325         }
1326 }
1327
1328 #undef phy_stacksave
1329 #undef phy_stackrestore
1330 #undef radio_stacksave
1331 #undef radio_stackrestore
1332 #undef ilt_stacksave
1333 #undef ilt_stackrestore
1334
1335 int b43legacy_radio_set_interference_mitigation(struct b43legacy_wldev *dev,
1336                                                 int mode)
1337 {
1338         struct b43legacy_phy *phy = &dev->phy;
1339         int currentmode;
1340
1341         if ((phy->type != B43legacy_PHYTYPE_G) ||
1342             (phy->rev == 0) || (!phy->gmode))
1343                 return -ENODEV;
1344
1345         phy->aci_wlan_automatic = false;
1346         switch (mode) {
1347         case B43legacy_RADIO_INTERFMODE_AUTOWLAN:
1348                 phy->aci_wlan_automatic = true;
1349                 if (phy->aci_enable)
1350                         mode = B43legacy_RADIO_INTERFMODE_MANUALWLAN;
1351                 else
1352                         mode = B43legacy_RADIO_INTERFMODE_NONE;
1353                 break;
1354         case B43legacy_RADIO_INTERFMODE_NONE:
1355         case B43legacy_RADIO_INTERFMODE_NONWLAN:
1356         case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
1357                 break;
1358         default:
1359                 return -EINVAL;
1360         }
1361
1362         currentmode = phy->interfmode;
1363         if (currentmode == mode)
1364                 return 0;
1365         if (currentmode != B43legacy_RADIO_INTERFMODE_NONE)
1366                 b43legacy_radio_interference_mitigation_disable(dev,
1367                                                                 currentmode);
1368
1369         if (mode == B43legacy_RADIO_INTERFMODE_NONE) {
1370                 phy->aci_enable = false;
1371                 phy->aci_hw_rssi = false;
1372         } else
1373                 b43legacy_radio_interference_mitigation_enable(dev, mode);
1374         phy->interfmode = mode;
1375
1376         return 0;
1377 }
1378
1379 u16 b43legacy_radio_calibrationvalue(struct b43legacy_wldev *dev)
1380 {
1381         u16 reg;
1382         u16 index;
1383         u16 ret;
1384
1385         reg = b43legacy_radio_read16(dev, 0x0060);
1386         index = (reg & 0x001E) >> 1;
1387         ret = rcc_table[index] << 1;
1388         ret |= (reg & 0x0001);
1389         ret |= 0x0020;
1390
1391         return ret;
1392 }
1393
1394 #define LPD(L, P, D)    (((L) << 2) | ((P) << 1) | ((D) << 0))
1395 static u16 b43legacy_get_812_value(struct b43legacy_wldev *dev, u8 lpd)
1396 {
1397         struct b43legacy_phy *phy = &dev->phy;
1398         u16 loop_or = 0;
1399         u16 adj_loopback_gain = phy->loopback_gain[0];
1400         u8 loop;
1401         u16 extern_lna_control;
1402
1403         if (!phy->gmode)
1404                 return 0;
1405         if (!has_loopback_gain(phy)) {
1406                 if (phy->rev < 7 || !(dev->dev->bus->sprom.boardflags_lo
1407                     & B43legacy_BFL_EXTLNA)) {
1408                         switch (lpd) {
1409                         case LPD(0, 1, 1):
1410                                 return 0x0FB2;
1411                         case LPD(0, 0, 1):
1412                                 return 0x00B2;
1413                         case LPD(1, 0, 1):
1414                                 return 0x30B2;
1415                         case LPD(1, 0, 0):
1416                                 return 0x30B3;
1417                         default:
1418                                 B43legacy_BUG_ON(1);
1419                         }
1420                 } else {
1421                         switch (lpd) {
1422                         case LPD(0, 1, 1):
1423                                 return 0x8FB2;
1424                         case LPD(0, 0, 1):
1425                                 return 0x80B2;
1426                         case LPD(1, 0, 1):
1427                                 return 0x20B2;
1428                         case LPD(1, 0, 0):
1429                                 return 0x20B3;
1430                         default:
1431                                 B43legacy_BUG_ON(1);
1432                         }
1433                 }
1434         } else {
1435                 if (phy->radio_rev == 8)
1436                         adj_loopback_gain += 0x003E;
1437                 else
1438                         adj_loopback_gain += 0x0026;
1439                 if (adj_loopback_gain >= 0x46) {
1440                         adj_loopback_gain -= 0x46;
1441                         extern_lna_control = 0x3000;
1442                 } else if (adj_loopback_gain >= 0x3A) {
1443                         adj_loopback_gain -= 0x3A;
1444                         extern_lna_control = 0x2000;
1445                 } else if (adj_loopback_gain >= 0x2E) {
1446                         adj_loopback_gain -= 0x2E;
1447                         extern_lna_control = 0x1000;
1448                 } else {
1449                         adj_loopback_gain -= 0x10;
1450                         extern_lna_control = 0x0000;
1451                 }
1452                 for (loop = 0; loop < 16; loop++) {
1453                         u16 tmp = adj_loopback_gain - 6 * loop;
1454                         if (tmp < 6)
1455                                 break;
1456                 }
1457
1458                 loop_or = (loop << 8) | extern_lna_control;
1459                 if (phy->rev >= 7 && dev->dev->bus->sprom.boardflags_lo
1460                     & B43legacy_BFL_EXTLNA) {
1461                         if (extern_lna_control)
1462                                 loop_or |= 0x8000;
1463                         switch (lpd) {
1464                         case LPD(0, 1, 1):
1465                                 return 0x8F92;
1466                         case LPD(0, 0, 1):
1467                                 return (0x8092 | loop_or);
1468                         case LPD(1, 0, 1):
1469                                 return (0x2092 | loop_or);
1470                         case LPD(1, 0, 0):
1471                                 return (0x2093 | loop_or);
1472                         default:
1473                                 B43legacy_BUG_ON(1);
1474                         }
1475                 } else {
1476                         switch (lpd) {
1477                         case LPD(0, 1, 1):
1478                                 return 0x0F92;
1479                         case LPD(0, 0, 1):
1480                         case LPD(1, 0, 1):
1481                                 return (0x0092 | loop_or);
1482                         case LPD(1, 0, 0):
1483                                 return (0x0093 | loop_or);
1484                         default:
1485                                 B43legacy_BUG_ON(1);
1486                         }
1487                 }
1488         }
1489         return 0;
1490 }
1491
1492 u16 b43legacy_radio_init2050(struct b43legacy_wldev *dev)
1493 {
1494         struct b43legacy_phy *phy = &dev->phy;
1495         u16 backup[21] = { 0 };
1496         u16 ret;
1497         u16 i;
1498         u16 j;
1499         u32 tmp1 = 0;
1500         u32 tmp2 = 0;
1501
1502         backup[0] = b43legacy_radio_read16(dev, 0x0043);
1503         backup[14] = b43legacy_radio_read16(dev, 0x0051);
1504         backup[15] = b43legacy_radio_read16(dev, 0x0052);
1505         backup[1] = b43legacy_phy_read(dev, 0x0015);
1506         backup[16] = b43legacy_phy_read(dev, 0x005A);
1507         backup[17] = b43legacy_phy_read(dev, 0x0059);
1508         backup[18] = b43legacy_phy_read(dev, 0x0058);
1509         if (phy->type == B43legacy_PHYTYPE_B) {
1510                 backup[2] = b43legacy_phy_read(dev, 0x0030);
1511                 backup[3] = b43legacy_read16(dev, 0x03EC);
1512                 b43legacy_phy_write(dev, 0x0030, 0x00FF);
1513                 b43legacy_write16(dev, 0x03EC, 0x3F3F);
1514         } else {
1515                 if (phy->gmode) {
1516                         backup[4] = b43legacy_phy_read(dev, 0x0811);
1517                         backup[5] = b43legacy_phy_read(dev, 0x0812);
1518                         backup[6] = b43legacy_phy_read(dev, 0x0814);
1519                         backup[7] = b43legacy_phy_read(dev, 0x0815);
1520                         backup[8] = b43legacy_phy_read(dev,
1521                                                        B43legacy_PHY_G_CRS);
1522                         backup[9] = b43legacy_phy_read(dev, 0x0802);
1523                         b43legacy_phy_write(dev, 0x0814,
1524                                             (b43legacy_phy_read(dev, 0x0814)
1525                                             | 0x0003));
1526                         b43legacy_phy_write(dev, 0x0815,
1527                                             (b43legacy_phy_read(dev, 0x0815)
1528                                             & 0xFFFC));
1529                         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1530                                             (b43legacy_phy_read(dev,
1531                                             B43legacy_PHY_G_CRS) & 0x7FFF));
1532                         b43legacy_phy_write(dev, 0x0802,
1533                                             (b43legacy_phy_read(dev, 0x0802)
1534                                             & 0xFFFC));
1535                         if (phy->rev > 1) { /* loopback gain enabled */
1536                                 backup[19] = b43legacy_phy_read(dev, 0x080F);
1537                                 backup[20] = b43legacy_phy_read(dev, 0x0810);
1538                                 if (phy->rev >= 3)
1539                                         b43legacy_phy_write(dev, 0x080F,
1540                                                             0xC020);
1541                                 else
1542                                         b43legacy_phy_write(dev, 0x080F,
1543                                                             0x8020);
1544                                 b43legacy_phy_write(dev, 0x0810, 0x0000);
1545                         }
1546                         b43legacy_phy_write(dev, 0x0812,
1547                                             b43legacy_get_812_value(dev,
1548                                             LPD(0, 1, 1)));
1549                         if (phy->rev < 7 ||
1550                             !(dev->dev->bus->sprom.boardflags_lo
1551                             & B43legacy_BFL_EXTLNA))
1552                                 b43legacy_phy_write(dev, 0x0811, 0x01B3);
1553                         else
1554                                 b43legacy_phy_write(dev, 0x0811, 0x09B3);
1555                 }
1556         }
1557         b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO,
1558                         (b43legacy_read16(dev, B43legacy_MMIO_PHY_RADIO)
1559                                           | 0x8000));
1560         backup[10] = b43legacy_phy_read(dev, 0x0035);
1561         b43legacy_phy_write(dev, 0x0035,
1562                             (b43legacy_phy_read(dev, 0x0035) & 0xFF7F));
1563         backup[11] = b43legacy_read16(dev, 0x03E6);
1564         backup[12] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
1565
1566         /* Initialization */
1567         if (phy->analog == 0)
1568                 b43legacy_write16(dev, 0x03E6, 0x0122);
1569         else {
1570                 if (phy->analog >= 2)
1571                         b43legacy_phy_write(dev, 0x0003,
1572                                             (b43legacy_phy_read(dev, 0x0003)
1573                                             & 0xFFBF) | 0x0040);
1574                 b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
1575                                   (b43legacy_read16(dev,
1576                                   B43legacy_MMIO_CHANNEL_EXT) | 0x2000));
1577         }
1578
1579         ret = b43legacy_radio_calibrationvalue(dev);
1580
1581         if (phy->type == B43legacy_PHYTYPE_B)
1582                 b43legacy_radio_write16(dev, 0x0078, 0x0026);
1583
1584         if (phy->gmode)
1585                 b43legacy_phy_write(dev, 0x0812,
1586                                     b43legacy_get_812_value(dev,
1587                                     LPD(0, 1, 1)));
1588         b43legacy_phy_write(dev, 0x0015, 0xBFAF);
1589         b43legacy_phy_write(dev, 0x002B, 0x1403);
1590         if (phy->gmode)
1591                 b43legacy_phy_write(dev, 0x0812,
1592                                     b43legacy_get_812_value(dev,
1593                                     LPD(0, 0, 1)));
1594         b43legacy_phy_write(dev, 0x0015, 0xBFA0);
1595         b43legacy_radio_write16(dev, 0x0051,
1596                                 (b43legacy_radio_read16(dev, 0x0051)
1597                                 | 0x0004));
1598         if (phy->radio_rev == 8)
1599                 b43legacy_radio_write16(dev, 0x0043, 0x001F);
1600         else {
1601                 b43legacy_radio_write16(dev, 0x0052, 0x0000);
1602                 b43legacy_radio_write16(dev, 0x0043,
1603                                         (b43legacy_radio_read16(dev, 0x0043)
1604                                         & 0xFFF0) | 0x0009);
1605         }
1606         b43legacy_phy_write(dev, 0x0058, 0x0000);
1607
1608         for (i = 0; i < 16; i++) {
1609                 b43legacy_phy_write(dev, 0x005A, 0x0480);
1610                 b43legacy_phy_write(dev, 0x0059, 0xC810);
1611                 b43legacy_phy_write(dev, 0x0058, 0x000D);
1612                 if (phy->gmode)
1613                         b43legacy_phy_write(dev, 0x0812,
1614                                             b43legacy_get_812_value(dev,
1615                                             LPD(1, 0, 1)));
1616                 b43legacy_phy_write(dev, 0x0015, 0xAFB0);
1617                 udelay(10);
1618                 if (phy->gmode)
1619                         b43legacy_phy_write(dev, 0x0812,
1620                                             b43legacy_get_812_value(dev,
1621                                             LPD(1, 0, 1)));
1622                 b43legacy_phy_write(dev, 0x0015, 0xEFB0);
1623                 udelay(10);
1624                 if (phy->gmode)
1625                         b43legacy_phy_write(dev, 0x0812,
1626                                             b43legacy_get_812_value(dev,
1627                                             LPD(1, 0, 0)));
1628                 b43legacy_phy_write(dev, 0x0015, 0xFFF0);
1629                 udelay(20);
1630                 tmp1 += b43legacy_phy_read(dev, 0x002D);
1631                 b43legacy_phy_write(dev, 0x0058, 0x0000);
1632                 if (phy->gmode)
1633                         b43legacy_phy_write(dev, 0x0812,
1634                                             b43legacy_get_812_value(dev,
1635                                             LPD(1, 0, 1)));
1636                 b43legacy_phy_write(dev, 0x0015, 0xAFB0);
1637         }
1638
1639         tmp1++;
1640         tmp1 >>= 9;
1641         udelay(10);
1642         b43legacy_phy_write(dev, 0x0058, 0x0000);
1643
1644         for (i = 0; i < 16; i++) {
1645                 b43legacy_radio_write16(dev, 0x0078, (flip_4bit(i) << 1)
1646                                         | 0x0020);
1647                 backup[13] = b43legacy_radio_read16(dev, 0x0078);
1648                 udelay(10);
1649                 for (j = 0; j < 16; j++) {
1650                         b43legacy_phy_write(dev, 0x005A, 0x0D80);
1651                         b43legacy_phy_write(dev, 0x0059, 0xC810);
1652                         b43legacy_phy_write(dev, 0x0058, 0x000D);
1653                         if (phy->gmode)
1654                                 b43legacy_phy_write(dev, 0x0812,
1655                                                     b43legacy_get_812_value(dev,
1656                                                     LPD(1, 0, 1)));
1657                         b43legacy_phy_write(dev, 0x0015, 0xAFB0);
1658                         udelay(10);
1659                         if (phy->gmode)
1660                                 b43legacy_phy_write(dev, 0x0812,
1661                                                     b43legacy_get_812_value(dev,
1662                                                     LPD(1, 0, 1)));
1663                         b43legacy_phy_write(dev, 0x0015, 0xEFB0);
1664                         udelay(10);
1665                         if (phy->gmode)
1666                                 b43legacy_phy_write(dev, 0x0812,
1667                                                     b43legacy_get_812_value(dev,
1668                                                     LPD(1, 0, 0)));
1669                         b43legacy_phy_write(dev, 0x0015, 0xFFF0);
1670                         udelay(10);
1671                         tmp2 += b43legacy_phy_read(dev, 0x002D);
1672                         b43legacy_phy_write(dev, 0x0058, 0x0000);
1673                         if (phy->gmode)
1674                                 b43legacy_phy_write(dev, 0x0812,
1675                                                     b43legacy_get_812_value(dev,
1676                                                     LPD(1, 0, 1)));
1677                         b43legacy_phy_write(dev, 0x0015, 0xAFB0);
1678                 }
1679                 tmp2++;
1680                 tmp2 >>= 8;
1681                 if (tmp1 < tmp2)
1682                         break;
1683         }
1684
1685         /* Restore the registers */
1686         b43legacy_phy_write(dev, 0x0015, backup[1]);
1687         b43legacy_radio_write16(dev, 0x0051, backup[14]);
1688         b43legacy_radio_write16(dev, 0x0052, backup[15]);
1689         b43legacy_radio_write16(dev, 0x0043, backup[0]);
1690         b43legacy_phy_write(dev, 0x005A, backup[16]);
1691         b43legacy_phy_write(dev, 0x0059, backup[17]);
1692         b43legacy_phy_write(dev, 0x0058, backup[18]);
1693         b43legacy_write16(dev, 0x03E6, backup[11]);
1694         if (phy->analog != 0)
1695                 b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, backup[12]);
1696         b43legacy_phy_write(dev, 0x0035, backup[10]);
1697         b43legacy_radio_selectchannel(dev, phy->channel, 1);
1698         if (phy->type == B43legacy_PHYTYPE_B) {
1699                 b43legacy_phy_write(dev, 0x0030, backup[2]);
1700                 b43legacy_write16(dev, 0x03EC, backup[3]);
1701         } else {
1702                 if (phy->gmode) {
1703                         b43legacy_write16(dev, B43legacy_MMIO_PHY_RADIO,
1704                                           (b43legacy_read16(dev,
1705                                           B43legacy_MMIO_PHY_RADIO) & 0x7FFF));
1706                         b43legacy_phy_write(dev, 0x0811, backup[4]);
1707                         b43legacy_phy_write(dev, 0x0812, backup[5]);
1708                         b43legacy_phy_write(dev, 0x0814, backup[6]);
1709                         b43legacy_phy_write(dev, 0x0815, backup[7]);
1710                         b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
1711                                             backup[8]);
1712                         b43legacy_phy_write(dev, 0x0802, backup[9]);
1713                         if (phy->rev > 1) {
1714                                 b43legacy_phy_write(dev, 0x080F, backup[19]);
1715                                 b43legacy_phy_write(dev, 0x0810, backup[20]);
1716                         }
1717                 }
1718         }
1719         if (i >= 15)
1720                 ret = backup[13];
1721
1722         return ret;
1723 }
1724
1725 static inline
1726 u16 freq_r3A_value(u16 frequency)
1727 {
1728         u16 value;
1729
1730         if (frequency < 5091)
1731                 value = 0x0040;
1732         else if (frequency < 5321)
1733                 value = 0x0000;
1734         else if (frequency < 5806)
1735                 value = 0x0080;
1736         else
1737                 value = 0x0040;
1738
1739         return value;
1740 }
1741
1742 int b43legacy_radio_selectchannel(struct b43legacy_wldev *dev,
1743                                   u8 channel,
1744                                   int synthetic_pu_workaround)
1745 {
1746         struct b43legacy_phy *phy = &dev->phy;
1747
1748         if (channel == 0xFF) {
1749                 switch (phy->type) {
1750                 case B43legacy_PHYTYPE_B:
1751                 case B43legacy_PHYTYPE_G:
1752                         channel = B43legacy_RADIO_DEFAULT_CHANNEL_BG;
1753                         break;
1754                 default:
1755                         B43legacy_WARN_ON(1);
1756                 }
1757         }
1758
1759 /* TODO: Check if channel is valid - return -EINVAL if not */
1760         if (synthetic_pu_workaround)
1761                 b43legacy_synth_pu_workaround(dev, channel);
1762
1763         b43legacy_write16(dev, B43legacy_MMIO_CHANNEL,
1764                           channel2freq_bg(channel));
1765
1766         if (channel == 14) {
1767                 if (dev->dev->bus->sprom.country_code == 5)   /* JAPAN) */
1768                         b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
1769                                               B43legacy_UCODEFLAGS_OFFSET,
1770                                               b43legacy_shm_read32(dev,
1771                                               B43legacy_SHM_SHARED,
1772                                               B43legacy_UCODEFLAGS_OFFSET)
1773                                               & ~(1 << 7));
1774                 else
1775                         b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
1776                                               B43legacy_UCODEFLAGS_OFFSET,
1777                                               b43legacy_shm_read32(dev,
1778                                               B43legacy_SHM_SHARED,
1779                                               B43legacy_UCODEFLAGS_OFFSET)
1780                                               | (1 << 7));
1781                 b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
1782                                   b43legacy_read16(dev,
1783                                   B43legacy_MMIO_CHANNEL_EXT) | (1 << 11));
1784         } else
1785                 b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
1786                                   b43legacy_read16(dev,
1787                                   B43legacy_MMIO_CHANNEL_EXT) & 0xF7BF);
1788
1789         phy->channel = channel;
1790         /*XXX: Using the longer of 2 timeouts (8000 vs 2000 usecs). Specs states
1791          *     that 2000 usecs might suffice. */
1792         msleep(8);
1793
1794         return 0;
1795 }
1796
1797 void b43legacy_radio_set_txantenna(struct b43legacy_wldev *dev, u32 val)
1798 {
1799         u16 tmp;
1800
1801         val <<= 8;
1802         tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0022) & 0xFCFF;
1803         b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0022, tmp | val);
1804         tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x03A8) & 0xFCFF;
1805         b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x03A8, tmp | val);
1806         tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0054) & 0xFCFF;
1807         b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0054, tmp | val);
1808 }
1809
1810 /* http://bcm-specs.sipsolutions.net/TX_Gain_Base_Band */
1811 static u16 b43legacy_get_txgain_base_band(u16 txpower)
1812 {
1813         u16 ret;
1814
1815         B43legacy_WARN_ON(txpower > 63);
1816
1817         if (txpower >= 54)
1818                 ret = 2;
1819         else if (txpower >= 49)
1820                 ret = 4;
1821         else if (txpower >= 44)
1822                 ret = 5;
1823         else
1824                 ret = 6;
1825
1826         return ret;
1827 }
1828
1829 /* http://bcm-specs.sipsolutions.net/TX_Gain_Radio_Frequency_Power_Amplifier */
1830 static u16 b43legacy_get_txgain_freq_power_amp(u16 txpower)
1831 {
1832         u16 ret;
1833
1834         B43legacy_WARN_ON(txpower > 63);
1835
1836         if (txpower >= 32)
1837                 ret = 0;
1838         else if (txpower >= 25)
1839                 ret = 1;
1840         else if (txpower >= 20)
1841                 ret = 2;
1842         else if (txpower >= 12)
1843                 ret = 3;
1844         else
1845                 ret = 4;
1846
1847         return ret;
1848 }
1849
1850 /* http://bcm-specs.sipsolutions.net/TX_Gain_Digital_Analog_Converter */
1851 static u16 b43legacy_get_txgain_dac(u16 txpower)
1852 {
1853         u16 ret;
1854
1855         B43legacy_WARN_ON(txpower > 63);
1856
1857         if (txpower >= 54)
1858                 ret = txpower - 53;
1859         else if (txpower >= 49)
1860                 ret = txpower - 42;
1861         else if (txpower >= 44)
1862                 ret = txpower - 37;
1863         else if (txpower >= 32)
1864                 ret = txpower - 32;
1865         else if (txpower >= 25)
1866                 ret = txpower - 20;
1867         else if (txpower >= 20)
1868                 ret = txpower - 13;
1869         else if (txpower >= 12)
1870                 ret = txpower - 8;
1871         else
1872                 ret = txpower;
1873
1874         return ret;
1875 }
1876
1877 void b43legacy_radio_set_txpower_a(struct b43legacy_wldev *dev, u16 txpower)
1878 {
1879         struct b43legacy_phy *phy = &dev->phy;
1880         u16 pamp;
1881         u16 base;
1882         u16 dac;
1883         u16 ilt;
1884
1885         txpower = clamp_val(txpower, 0, 63);
1886
1887         pamp = b43legacy_get_txgain_freq_power_amp(txpower);
1888         pamp <<= 5;
1889         pamp &= 0x00E0;
1890         b43legacy_phy_write(dev, 0x0019, pamp);
1891
1892         base = b43legacy_get_txgain_base_band(txpower);
1893         base &= 0x000F;
1894         b43legacy_phy_write(dev, 0x0017, base | 0x0020);
1895
1896         ilt = b43legacy_ilt_read(dev, 0x3001);
1897         ilt &= 0x0007;
1898
1899         dac = b43legacy_get_txgain_dac(txpower);
1900         dac <<= 3;
1901         dac |= ilt;
1902
1903         b43legacy_ilt_write(dev, 0x3001, dac);
1904
1905         phy->txpwr_offset = txpower;
1906
1907         /* TODO: FuncPlaceholder (Adjust BB loft cancel) */
1908 }
1909
1910 void b43legacy_radio_set_txpower_bg(struct b43legacy_wldev *dev,
1911                                     u16 baseband_attenuation,
1912                                     u16 radio_attenuation,
1913                                     u16 txpower)
1914 {
1915         struct b43legacy_phy *phy = &dev->phy;
1916
1917         if (baseband_attenuation == 0xFFFF)
1918                 baseband_attenuation = phy->bbatt;
1919         if (radio_attenuation == 0xFFFF)
1920                 radio_attenuation = phy->rfatt;
1921         if (txpower == 0xFFFF)
1922                 txpower = phy->txctl1;
1923         phy->bbatt = baseband_attenuation;
1924         phy->rfatt = radio_attenuation;
1925         phy->txctl1 = txpower;
1926
1927         B43legacy_WARN_ON(baseband_attenuation > 11);
1928         if (phy->radio_rev < 6)
1929                 B43legacy_WARN_ON(radio_attenuation > 9);
1930         else
1931                 B43legacy_WARN_ON(radio_attenuation > 31);
1932         B43legacy_WARN_ON(txpower > 7);
1933
1934         b43legacy_phy_set_baseband_attenuation(dev, baseband_attenuation);
1935         b43legacy_radio_write16(dev, 0x0043, radio_attenuation);
1936         b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0064,
1937                               radio_attenuation);
1938         if (phy->radio_ver == 0x2050)
1939                 b43legacy_radio_write16(dev, 0x0052,
1940                                         (b43legacy_radio_read16(dev, 0x0052)
1941                                         & ~0x0070) | ((txpower << 4) & 0x0070));
1942         /* FIXME: The spec is very weird and unclear here. */
1943         if (phy->type == B43legacy_PHYTYPE_G)
1944                 b43legacy_phy_lo_adjust(dev, 0);
1945 }
1946
1947 u16 b43legacy_default_baseband_attenuation(struct b43legacy_wldev *dev)
1948 {
1949         struct b43legacy_phy *phy = &dev->phy;
1950
1951         if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
1952                 return 0;
1953         return 2;
1954 }
1955
1956 u16 b43legacy_default_radio_attenuation(struct b43legacy_wldev *dev)
1957 {
1958         struct b43legacy_phy *phy = &dev->phy;
1959         u16 att = 0xFFFF;
1960
1961         switch (phy->radio_ver) {
1962         case 0x2053:
1963                 switch (phy->radio_rev) {
1964                 case 1:
1965                         att = 6;
1966                         break;
1967                 }
1968                 break;
1969         case 0x2050:
1970                 switch (phy->radio_rev) {
1971                 case 0:
1972                         att = 5;
1973                         break;
1974                 case 1:
1975                         if (phy->type == B43legacy_PHYTYPE_G) {
1976                                 if (is_bcm_board_vendor(dev) &&
1977                                     dev->dev->bus->boardinfo.type == 0x421 &&
1978                                     dev->dev->bus->sprom.board_rev >= 30)
1979                                         att = 3;
1980                                 else if (is_bcm_board_vendor(dev) &&
1981                                          dev->dev->bus->boardinfo.type == 0x416)
1982                                         att = 3;
1983                                 else
1984                                         att = 1;
1985                         } else {
1986                                 if (is_bcm_board_vendor(dev) &&
1987                                     dev->dev->bus->boardinfo.type == 0x421 &&
1988                                     dev->dev->bus->sprom.board_rev >= 30)
1989                                         att = 7;
1990                                 else
1991                                         att = 6;
1992                         }
1993                         break;
1994                 case 2:
1995                         if (phy->type == B43legacy_PHYTYPE_G) {
1996                                 if (is_bcm_board_vendor(dev) &&
1997                                     dev->dev->bus->boardinfo.type == 0x421 &&
1998                                     dev->dev->bus->sprom.board_rev >= 30)
1999                                         att = 3;
2000                                 else if (is_bcm_board_vendor(dev) &&
2001                                          dev->dev->bus->boardinfo.type ==
2002                                          0x416)
2003                                         att = 5;
2004                                 else if (dev->dev->bus->chip_id == 0x4320)
2005                                         att = 4;
2006                                 else
2007                                         att = 3;
2008                         } else
2009                                 att = 6;
2010                         break;
2011                 case 3:
2012                         att = 5;
2013                         break;
2014                 case 4:
2015                 case 5:
2016                         att = 1;
2017                         break;
2018                 case 6:
2019                 case 7:
2020                         att = 5;
2021                         break;
2022                 case 8:
2023                         att = 0x1A;
2024                         break;
2025                 case 9:
2026                 default:
2027                         att = 5;
2028                 }
2029         }
2030         if (is_bcm_board_vendor(dev) &&
2031             dev->dev->bus->boardinfo.type == 0x421) {
2032                 if (dev->dev->bus->sprom.board_rev < 0x43)
2033                         att = 2;
2034                 else if (dev->dev->bus->sprom.board_rev < 0x51)
2035                         att = 3;
2036         }
2037         if (att == 0xFFFF)
2038                 att = 5;
2039
2040         return att;
2041 }
2042
2043 u16 b43legacy_default_txctl1(struct b43legacy_wldev *dev)
2044 {
2045         struct b43legacy_phy *phy = &dev->phy;
2046
2047         if (phy->radio_ver != 0x2050)
2048                 return 0;
2049         if (phy->radio_rev == 1)
2050                 return 3;
2051         if (phy->radio_rev < 6)
2052                 return 2;
2053         if (phy->radio_rev == 8)
2054                 return 1;
2055         return 0;
2056 }
2057
2058 void b43legacy_radio_turn_on(struct b43legacy_wldev *dev)
2059 {
2060         struct b43legacy_phy *phy = &dev->phy;
2061         int err;
2062         u8 channel;
2063
2064         might_sleep();
2065
2066         if (phy->radio_on)
2067                 return;
2068
2069         switch (phy->type) {
2070         case B43legacy_PHYTYPE_B:
2071         case B43legacy_PHYTYPE_G:
2072                 b43legacy_phy_write(dev, 0x0015, 0x8000);
2073                 b43legacy_phy_write(dev, 0x0015, 0xCC00);
2074                 b43legacy_phy_write(dev, 0x0015,
2075                                     (phy->gmode ? 0x00C0 : 0x0000));
2076                 if (phy->radio_off_context.valid) {
2077                         /* Restore the RFover values. */
2078                         b43legacy_phy_write(dev, B43legacy_PHY_RFOVER,
2079                                             phy->radio_off_context.rfover);
2080                         b43legacy_phy_write(dev, B43legacy_PHY_RFOVERVAL,
2081                                             phy->radio_off_context.rfoverval);
2082                         phy->radio_off_context.valid = false;
2083                 }
2084                 channel = phy->channel;
2085                 err = b43legacy_radio_selectchannel(dev,
2086                                         B43legacy_RADIO_DEFAULT_CHANNEL_BG, 1);
2087                 err |= b43legacy_radio_selectchannel(dev, channel, 0);
2088                 B43legacy_WARN_ON(err);
2089                 break;
2090         default:
2091                 B43legacy_BUG_ON(1);
2092         }
2093         phy->radio_on = true;
2094 }
2095
2096 void b43legacy_radio_turn_off(struct b43legacy_wldev *dev, bool force)
2097 {
2098         struct b43legacy_phy *phy = &dev->phy;
2099
2100         if (!phy->radio_on && !force)
2101                 return;
2102
2103         if (phy->type == B43legacy_PHYTYPE_G && dev->dev->id.revision >= 5) {
2104                 u16 rfover, rfoverval;
2105
2106                 rfover = b43legacy_phy_read(dev, B43legacy_PHY_RFOVER);
2107                 rfoverval = b43legacy_phy_read(dev, B43legacy_PHY_RFOVERVAL);
2108                 if (!force) {
2109                         phy->radio_off_context.rfover = rfover;
2110                         phy->radio_off_context.rfoverval = rfoverval;
2111                         phy->radio_off_context.valid = true;
2112                 }
2113                 b43legacy_phy_write(dev, B43legacy_PHY_RFOVER, rfover | 0x008C);
2114                 b43legacy_phy_write(dev, B43legacy_PHY_RFOVERVAL,
2115                                     rfoverval & 0xFF73);
2116         } else
2117                 b43legacy_phy_write(dev, 0x0015, 0xAA00);
2118         phy->radio_on = false;
2119         b43legacydbg(dev->wl, "Radio initialized\n");
2120 }
2121
2122 void b43legacy_radio_clear_tssi(struct b43legacy_wldev *dev)
2123 {
2124         struct b43legacy_phy *phy = &dev->phy;
2125
2126         switch (phy->type) {
2127         case B43legacy_PHYTYPE_B:
2128         case B43legacy_PHYTYPE_G:
2129                 b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0058,
2130                                       0x7F7F);
2131                 b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x005a,
2132                                       0x7F7F);
2133                 b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0070,
2134                                       0x7F7F);
2135                 b43legacy_shm_write16(dev, B43legacy_SHM_SHARED, 0x0072,
2136                                       0x7F7F);
2137                 break;
2138         }
2139 }