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