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