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