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