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

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