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