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