1	// SPDX-License-Identifier: GPL-2.0-or-later
2	/*
3
4	Broadcom B43 wireless driver
5	IEEE 802.11g PHY driver
6
7	Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
8	Copyright (c) 2005-2007 Stefano Brivio <stefano.brivio@polimi.it>
9	Copyright (c) 2005-2008 Michael Buesch <m@bues.ch>
10	Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
11	Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
12
13
14	*/
15
16	#include "b43.h"
17	#include "phy_g.h"
18	#include "phy_common.h"
19	#include "lo.h"
20	#include "main.h"
21	#include "wa.h"
22
23	#include <linux/bitrev.h>
24	#include <linux/slab.h>
25
26
27	static const s8 b43_tssi2dbm_g_table[] = {
28	77, 77, 77, 76,
29	76, 76, 75, 75,
30	74, 74, 73, 73,
31	73, 72, 72, 71,
32	71, 70, 70, 69,
33	68, 68, 67, 67,
34	66, 65, 65, 64,
35	63, 63, 62, 61,
36	60, 59, 58, 57,
37	56, 55, 54, 53,
38	52, 50, 49, 47,
39	45, 43, 40, 37,
40	33, 28, 22, 14,
41	5, -7, -20, -20,
42	-20, -20, -20, -20,
43	-20, -20, -20, -20,
44	};
45
46	static const u8 b43_radio_channel_codes_bg[] = {
47	12, 17, 22, 27,
48	32, 37, 42, 47,
49	52, 57, 62, 67,
50	72, 84,
51	};
52
53
54	static void b43_calc_nrssi_threshold(struct b43_wldev *dev);
55
56
57	#define bitrev4(tmp) (bitrev8(tmp) >> 4)
58
59
60	/* Get the freq, as it has to be written to the device. */
61	static inline u16 channel2freq_bg(u8 channel)
62	{
63	B43_WARN_ON(!(channel >= 1 && channel <= 14));
64
65	return b43_radio_channel_codes_bg[channel - 1];
66	}
67
68	static void generate_rfatt_list(struct b43_wldev *dev,
69	struct b43_rfatt_list *list)
70	{
71	struct b43_phy *phy = &dev->phy;
72
73	/* APHY.rev < 5 || GPHY.rev < 6 */
74	static const struct b43_rfatt rfatt_0[] = {
75	{.att = 3,.with_padmix = 0,},
76	{.att = 1,.with_padmix = 0,},
77	{.att = 5,.with_padmix = 0,},
78	{.att = 7,.with_padmix = 0,},
79	{.att = 9,.with_padmix = 0,},
80	{.att = 2,.with_padmix = 0,},
81	{.att = 0,.with_padmix = 0,},
82	{.att = 4,.with_padmix = 0,},
83	{.att = 6,.with_padmix = 0,},
84	{.att = 8,.with_padmix = 0,},
85	{.att = 1,.with_padmix = 1,},
86	{.att = 2,.with_padmix = 1,},
87	{.att = 3,.with_padmix = 1,},
88	{.att = 4,.with_padmix = 1,},
89	};
90	/* Radio.rev == 8 && Radio.version == 0x2050 */
91	static const struct b43_rfatt rfatt_1[] = {
92	{.att = 2,.with_padmix = 1,},
93	{.att = 4,.with_padmix = 1,},
94	{.att = 6,.with_padmix = 1,},
95	{.att = 8,.with_padmix = 1,},
96	{.att = 10,.with_padmix = 1,},
97	{.att = 12,.with_padmix = 1,},
98	{.att = 14,.with_padmix = 1,},
99	};
100	/* Otherwise */
101	static const struct b43_rfatt rfatt_2[] = {
102	{.att = 0,.with_padmix = 1,},
103	{.att = 2,.with_padmix = 1,},
104	{.att = 4,.with_padmix = 1,},
105	{.att = 6,.with_padmix = 1,},
106	{.att = 8,.with_padmix = 1,},
107	{.att = 9,.with_padmix = 1,},
108	{.att = 9,.with_padmix = 1,},
109	};
110
111	if (!b43_has_hardware_pctl(dev)) {
112	/* Software pctl */
113	list->list = rfatt_0;
114	list->len = ARRAY_SIZE(rfatt_0);
115	list->min_val = 0;
116	list->max_val = 9;
117	return;
118	}
119	if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
120	/* Hardware pctl */
121	list->list = rfatt_1;
122	list->len = ARRAY_SIZE(rfatt_1);
123	list->min_val = 0;
124	list->max_val = 14;
125	return;
126	}
127	/* Hardware pctl */
128	list->list = rfatt_2;
129	list->len = ARRAY_SIZE(rfatt_2);
130	list->min_val = 0;
131	list->max_val = 9;
132	}
133
134	static void generate_bbatt_list(struct b43_wldev *dev,
135	struct b43_bbatt_list *list)
136	{
137	static const struct b43_bbatt bbatt_0[] = {
138	{.att = 0,},
139	{.att = 1,},
140	{.att = 2,},
141	{.att = 3,},
142	{.att = 4,},
143	{.att = 5,},
144	{.att = 6,},
145	{.att = 7,},
146	{.att = 8,},
147	};
148
149	list->list = bbatt_0;
150	list->len = ARRAY_SIZE(bbatt_0);
151	list->min_val = 0;
152	list->max_val = 8;
153	}
154
155	static void b43_shm_clear_tssi(struct b43_wldev *dev)
156	{
157	b43_shm_write16(dev, routing: B43_SHM_SHARED, offset: 0x0058, value: 0x7F7F);
158	b43_shm_write16(dev, routing: B43_SHM_SHARED, offset: 0x005a, value: 0x7F7F);
159	b43_shm_write16(dev, routing: B43_SHM_SHARED, offset: 0x0070, value: 0x7F7F);
160	b43_shm_write16(dev, routing: B43_SHM_SHARED, offset: 0x0072, value: 0x7F7F);
161	}
162
163	/* Synthetic PU workaround */
164	static void b43_synth_pu_workaround(struct b43_wldev *dev, u8 channel)
165	{
166	struct b43_phy *phy = &dev->phy;
167
168	might_sleep();
169
170	if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6) {
171	/* We do not need the workaround. */
172	return;
173	}
174
175	if (channel <= 10) {
176	b43_write16(dev, B43_MMIO_CHANNEL,
177	value: channel2freq_bg(channel: channel + 4));
178	} else {
179	b43_write16(dev, B43_MMIO_CHANNEL, value: channel2freq_bg(channel: 1));
180	}
181	msleep(msecs: 1);
182	b43_write16(dev, B43_MMIO_CHANNEL, value: channel2freq_bg(channel));
183	}
184
185	/* Set the baseband attenuation value on chip. */
186	void b43_gphy_set_baseband_attenuation(struct b43_wldev *dev,
187	u16 baseband_attenuation)
188	{
189	struct b43_phy *phy = &dev->phy;
190
191	if (phy->analog == 0) {
192	b43_write16(dev, B43_MMIO_PHY0, value: (b43_read16(dev, B43_MMIO_PHY0)
193	& 0xFFF0) |
194	baseband_attenuation);
195	} else if (phy->analog > 1) {
196	b43_phy_maskset(dev, B43_PHY_DACCTL, mask: 0xFFC3, set: (baseband_attenuation << 2));
197	} else {
198	b43_phy_maskset(dev, B43_PHY_DACCTL, mask: 0xFF87, set: (baseband_attenuation << 3));
199	}
200	}
201
202	/* Adjust the transmission power output (G-PHY) */
203	static void b43_set_txpower_g(struct b43_wldev *dev,
204	const struct b43_bbatt *bbatt,
205	const struct b43_rfatt *rfatt, u8 tx_control)
206	{
207	struct b43_phy *phy = &dev->phy;
208	struct b43_phy_g *gphy = phy->g;
209	struct b43_txpower_lo_control *lo = gphy->lo_control;
210	u16 bb, rf;
211	u16 tx_bias, tx_magn;
212
213	bb = bbatt->att;
214	rf = rfatt->att;
215	tx_bias = lo->tx_bias;
216	tx_magn = lo->tx_magn;
217	if (unlikely(tx_bias == 0xFF))
218	tx_bias = 0;
219
220	/* Save the values for later. Use memmove, because it's valid
221	* to pass &gphy->rfatt as rfatt pointer argument. Same for bbatt. */
222	gphy->tx_control = tx_control;
223	memmove(&gphy->rfatt, rfatt, sizeof(*rfatt));
224	gphy->rfatt.with_padmix = !!(tx_control & B43_TXCTL_TXMIX);
225	memmove(&gphy->bbatt, bbatt, sizeof(*bbatt));
226
227	if (b43_debug(dev, feature: B43_DBG_XMITPOWER)) {
228	b43dbg(wl: dev->wl, fmt: "Tuning TX-power to bbatt(%u), "
229	"rfatt(%u), tx_control(0x%02X), "
230	"tx_bias(0x%02X), tx_magn(0x%02X)\n",
231	bb, rf, tx_control, tx_bias, tx_magn);
232	}
233
234	b43_gphy_set_baseband_attenuation(dev, baseband_attenuation: bb);
235	b43_shm_write16(dev, routing: B43_SHM_SHARED, B43_SHM_SH_RFATT, value: rf);
236	if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
237	b43_radio_write16(dev, reg: 0x43,
238	value: (rf & 0x000F) | (tx_control & 0x0070));
239	} else {
240	b43_radio_maskset(dev, offset: 0x43, mask: 0xFFF0, set: (rf & 0x000F));
241	b43_radio_maskset(dev, offset: 0x52, mask: ~0x0070, set: (tx_control & 0x0070));
242	}
243	if (has_tx_magnification(phy)) {
244	b43_radio_write16(dev, reg: 0x52, value: tx_magn | tx_bias);
245	} else {
246	b43_radio_maskset(dev, offset: 0x52, mask: 0xFFF0, set: (tx_bias & 0x000F));
247	}
248	b43_lo_g_adjust(dev);
249	}
250
251	/* GPHY_TSSI_Power_Lookup_Table_Init */
252	static void b43_gphy_tssi_power_lt_init(struct b43_wldev *dev)
253	{
254	struct b43_phy_g *gphy = dev->phy.g;
255	int i;
256	u16 value;
257
258	for (i = 0; i < 32; i++)
259	b43_ofdmtab_write16(dev, table: 0x3C20, offset: i, value: gphy->tssi2dbm[i]);
260	for (i = 32; i < 64; i++)
261	b43_ofdmtab_write16(dev, table: 0x3C00, offset: i - 32, value: gphy->tssi2dbm[i]);
262	for (i = 0; i < 64; i += 2) {
263	value = (u16) gphy->tssi2dbm[i];
264	value |= ((u16) gphy->tssi2dbm[i + 1]) << 8;
265	b43_phy_write(dev, reg: 0x380 + (i / 2), value);
266	}
267	}
268
269	/* GPHY_Gain_Lookup_Table_Init */
270	static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
271	{
272	struct b43_phy *phy = &dev->phy;
273	struct b43_phy_g *gphy = phy->g;
274	struct b43_txpower_lo_control *lo = gphy->lo_control;
275	u16 nr_written = 0;
276	u16 tmp;
277	u8 rf, bb;
278
279	for (rf = 0; rf < lo->rfatt_list.len; rf++) {
280	for (bb = 0; bb < lo->bbatt_list.len; bb++) {
281	if (nr_written >= 0x40)
282	return;
283	tmp = lo->bbatt_list.list[bb].att;
284	tmp <<= 8;
285	if (phy->radio_rev == 8)
286	tmp |= 0x50;
287	else
288	tmp |= 0x40;
289	tmp |= lo->rfatt_list.list[rf].att;
290	b43_phy_write(dev, reg: 0x3C0 + nr_written, value: tmp);
291	nr_written++;
292	}
293	}
294	}
295
296	static void b43_set_all_gains(struct b43_wldev *dev,
297	s16 first, s16 second, s16 third)
298	{
299	struct b43_phy *phy = &dev->phy;
300	u16 i;
301	u16 start = 0x08, end = 0x18;
302	u16 tmp;
303	u16 table;
304
305	if (phy->rev <= 1) {
306	start = 0x10;
307	end = 0x20;
308	}
309
310	table = B43_OFDMTAB_GAINX;
311	if (phy->rev <= 1)
312	table = B43_OFDMTAB_GAINX_R1;
313	for (i = 0; i < 4; i++)
314	b43_ofdmtab_write16(dev, table, offset: i, value: first);
315
316	for (i = start; i < end; i++)
317	b43_ofdmtab_write16(dev, table, offset: i, value: second);
318
319	if (third != -1) {
320	tmp = ((u16) third << 14) | ((u16) third << 6);
321	b43_phy_maskset(dev, offset: 0x04A0, mask: 0xBFBF, set: tmp);
322	b43_phy_maskset(dev, offset: 0x04A1, mask: 0xBFBF, set: tmp);
323	b43_phy_maskset(dev, offset: 0x04A2, mask: 0xBFBF, set: tmp);
324	}
325	b43_dummy_transmission(dev, ofdm: false, pa_on: true);
326	}
327
328	static void b43_set_original_gains(struct b43_wldev *dev)
329	{
330	struct b43_phy *phy = &dev->phy;
331	u16 i, tmp;
332	u16 table;
333	u16 start = 0x0008, end = 0x0018;
334
335	if (phy->rev <= 1) {
336	start = 0x0010;
337	end = 0x0020;
338	}
339
340	table = B43_OFDMTAB_GAINX;
341	if (phy->rev <= 1)
342	table = B43_OFDMTAB_GAINX_R1;
343	for (i = 0; i < 4; i++) {
344	tmp = (i & 0xFFFC);
345	tmp |= (i & 0x0001) << 1;
346	tmp |= (i & 0x0002) >> 1;
347
348	b43_ofdmtab_write16(dev, table, offset: i, value: tmp);
349	}
350
351	for (i = start; i < end; i++)
352	b43_ofdmtab_write16(dev, table, offset: i, value: i - start);
353
354	b43_phy_maskset(dev, offset: 0x04A0, mask: 0xBFBF, set: 0x4040);
355	b43_phy_maskset(dev, offset: 0x04A1, mask: 0xBFBF, set: 0x4040);
356	b43_phy_maskset(dev, offset: 0x04A2, mask: 0xBFBF, set: 0x4000);
357	b43_dummy_transmission(dev, ofdm: false, pa_on: true);
358	}
359
360	/* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
361	static void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val)
362	{
363	b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, value: offset);
364	b43_phy_write(dev, B43_PHY_NRSSILT_DATA, value: (u16) val);
365	}
366
367	/* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
368	static s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset)
369	{
370	u16 val;
371
372	b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, value: offset);
373	val = b43_phy_read(dev, B43_PHY_NRSSILT_DATA);
374
375	return (s16) val;
376	}
377
378	/* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
379	static void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val)
380	{
381	u16 i;
382	s16 tmp;
383
384	for (i = 0; i < 64; i++) {
385	tmp = b43_nrssi_hw_read(dev, offset: i);
386	tmp -= val;
387	tmp = clamp_val(tmp, -32, 31);
388	b43_nrssi_hw_write(dev, offset: i, val: tmp);
389	}
390	}
391
392	/* https://bcm-specs.sipsolutions.net/NRSSILookupTable */
393	static void b43_nrssi_mem_update(struct b43_wldev *dev)
394	{
395	struct b43_phy_g *gphy = dev->phy.g;
396	s16 i, delta;
397	s32 tmp;
398
399	delta = 0x1F - gphy->nrssi[0];
400	for (i = 0; i < 64; i++) {
401	tmp = (i - delta) * gphy->nrssislope;
402	tmp /= 0x10000;
403	tmp += 0x3A;
404	tmp = clamp_val(tmp, 0, 0x3F);
405	gphy->nrssi_lt[i] = tmp;
406	}
407	}
408
409	static void b43_calc_nrssi_offset(struct b43_wldev *dev)
410	{
411	struct b43_phy *phy = &dev->phy;
412	u16 backup[20] = { 0 };
413	s16 v47F;
414	u16 i;
415	u16 saved = 0xFFFF;
416
417	backup[0] = b43_phy_read(dev, reg: 0x0001);
418	backup[1] = b43_phy_read(dev, reg: 0x0811);
419	backup[2] = b43_phy_read(dev, reg: 0x0812);
420	if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */
421	backup[3] = b43_phy_read(dev, reg: 0x0814);
422	backup[4] = b43_phy_read(dev, reg: 0x0815);
423	}
424	backup[5] = b43_phy_read(dev, reg: 0x005A);
425	backup[6] = b43_phy_read(dev, reg: 0x0059);
426	backup[7] = b43_phy_read(dev, reg: 0x0058);
427	backup[8] = b43_phy_read(dev, reg: 0x000A);
428	backup[9] = b43_phy_read(dev, reg: 0x0003);
429	backup[10] = b43_radio_read16(dev, reg: 0x007A);
430	backup[11] = b43_radio_read16(dev, reg: 0x0043);
431
432	b43_phy_mask(dev, offset: 0x0429, mask: 0x7FFF);
433	b43_phy_maskset(dev, offset: 0x0001, mask: 0x3FFF, set: 0x4000);
434	b43_phy_set(dev, offset: 0x0811, set: 0x000C);
435	b43_phy_maskset(dev, offset: 0x0812, mask: 0xFFF3, set: 0x0004);
436	b43_phy_mask(dev, offset: 0x0802, mask: ~(0x1 | 0x2));
437	if (phy->rev >= 6) {
438	backup[12] = b43_phy_read(dev, reg: 0x002E);
439	backup[13] = b43_phy_read(dev, reg: 0x002F);
440	backup[14] = b43_phy_read(dev, reg: 0x080F);
441	backup[15] = b43_phy_read(dev, reg: 0x0810);
442	backup[16] = b43_phy_read(dev, reg: 0x0801);
443	backup[17] = b43_phy_read(dev, reg: 0x0060);
444	backup[18] = b43_phy_read(dev, reg: 0x0014);
445	backup[19] = b43_phy_read(dev, reg: 0x0478);
446
447	b43_phy_write(dev, reg: 0x002E, value: 0);
448	b43_phy_write(dev, reg: 0x002F, value: 0);
449	b43_phy_write(dev, reg: 0x080F, value: 0);
450	b43_phy_write(dev, reg: 0x0810, value: 0);
451	b43_phy_set(dev, offset: 0x0478, set: 0x0100);
452	b43_phy_set(dev, offset: 0x0801, set: 0x0040);
453	b43_phy_set(dev, offset: 0x0060, set: 0x0040);
454	b43_phy_set(dev, offset: 0x0014, set: 0x0200);
455	}
456	b43_radio_set(dev, offset: 0x007A, set: 0x0070);
457	b43_radio_set(dev, offset: 0x007A, set: 0x0080);
458	udelay(30);
459
460	v47F = (s16) ((b43_phy_read(dev, reg: 0x047F) >> 8) & 0x003F);
461	if (v47F >= 0x20)
462	v47F -= 0x40;
463	if (v47F == 31) {
464	for (i = 7; i >= 4; i--) {
465	b43_radio_write16(dev, reg: 0x007B, value: i);
466	udelay(20);
467	v47F =
468	(s16) ((b43_phy_read(dev, reg: 0x047F) >> 8) & 0x003F);
469	if (v47F >= 0x20)
470	v47F -= 0x40;
471	if (v47F < 31 && saved == 0xFFFF)
472	saved = i;
473	}
474	if (saved == 0xFFFF)
475	saved = 4;
476	} else {
477	b43_radio_mask(dev, offset: 0x007A, mask: 0x007F);
478	if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */
479	b43_phy_set(dev, offset: 0x0814, set: 0x0001);
480	b43_phy_mask(dev, offset: 0x0815, mask: 0xFFFE);
481	}
482	b43_phy_set(dev, offset: 0x0811, set: 0x000C);
483	b43_phy_set(dev, offset: 0x0812, set: 0x000C);
484	b43_phy_set(dev, offset: 0x0811, set: 0x0030);
485	b43_phy_set(dev, offset: 0x0812, set: 0x0030);
486	b43_phy_write(dev, reg: 0x005A, value: 0x0480);
487	b43_phy_write(dev, reg: 0x0059, value: 0x0810);
488	b43_phy_write(dev, reg: 0x0058, value: 0x000D);
489	if (phy->rev == 0) {
490	b43_phy_write(dev, reg: 0x0003, value: 0x0122);
491	} else {
492	b43_phy_set(dev, offset: 0x000A, set: 0x2000);
493	}
494	if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */
495	b43_phy_set(dev, offset: 0x0814, set: 0x0004);
496	b43_phy_mask(dev, offset: 0x0815, mask: 0xFFFB);
497	}
498	b43_phy_maskset(dev, offset: 0x0003, mask: 0xFF9F, set: 0x0040);
499	b43_radio_set(dev, offset: 0x007A, set: 0x000F);
500	b43_set_all_gains(dev, first: 3, second: 0, third: 1);
501	b43_radio_maskset(dev, offset: 0x0043, mask: 0x00F0, set: 0x000F);
502	udelay(30);
503	v47F = (s16) ((b43_phy_read(dev, reg: 0x047F) >> 8) & 0x003F);
504	if (v47F >= 0x20)
505	v47F -= 0x40;
506	if (v47F == -32) {
507	for (i = 0; i < 4; i++) {
508	b43_radio_write16(dev, reg: 0x007B, value: i);
509	udelay(20);
510	v47F =
511	(s16) ((b43_phy_read(dev, reg: 0x047F) >> 8) &
512	0x003F);
513	if (v47F >= 0x20)
514	v47F -= 0x40;
515	if (v47F > -31 && saved == 0xFFFF)
516	saved = i;
517	}
518	if (saved == 0xFFFF)
519	saved = 3;
520	} else
521	saved = 0;
522	}
523	b43_radio_write16(dev, reg: 0x007B, value: saved);
524
525	if (phy->rev >= 6) {
526	b43_phy_write(dev, reg: 0x002E, value: backup[12]);
527	b43_phy_write(dev, reg: 0x002F, value: backup[13]);
528	b43_phy_write(dev, reg: 0x080F, value: backup[14]);
529	b43_phy_write(dev, reg: 0x0810, value: backup[15]);
530	}
531	if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */
532	b43_phy_write(dev, reg: 0x0814, value: backup[3]);
533	b43_phy_write(dev, reg: 0x0815, value: backup[4]);
534	}
535	b43_phy_write(dev, reg: 0x005A, value: backup[5]);
536	b43_phy_write(dev, reg: 0x0059, value: backup[6]);
537	b43_phy_write(dev, reg: 0x0058, value: backup[7]);
538	b43_phy_write(dev, reg: 0x000A, value: backup[8]);
539	b43_phy_write(dev, reg: 0x0003, value: backup[9]);
540	b43_radio_write16(dev, reg: 0x0043, value: backup[11]);
541	b43_radio_write16(dev, reg: 0x007A, value: backup[10]);
542	b43_phy_write(dev, reg: 0x0802, value: b43_phy_read(dev, reg: 0x0802) | 0x1 | 0x2);
543	b43_phy_set(dev, offset: 0x0429, set: 0x8000);
544	b43_set_original_gains(dev);
545	if (phy->rev >= 6) {
546	b43_phy_write(dev, reg: 0x0801, value: backup[16]);
547	b43_phy_write(dev, reg: 0x0060, value: backup[17]);
548	b43_phy_write(dev, reg: 0x0014, value: backup[18]);
549	b43_phy_write(dev, reg: 0x0478, value: backup[19]);
550	}
551	b43_phy_write(dev, reg: 0x0001, value: backup[0]);
552	b43_phy_write(dev, reg: 0x0812, value: backup[2]);
553	b43_phy_write(dev, reg: 0x0811, value: backup[1]);
554	}
555
556	static void b43_calc_nrssi_slope(struct b43_wldev *dev)
557	{
558	struct b43_phy *phy = &dev->phy;
559	struct b43_phy_g *gphy = phy->g;
560	u16 backup[18] = { 0 };
561	u16 tmp;
562	s16 nrssi0, nrssi1;
563
564	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
565
566	if (phy->radio_rev >= 9)
567	return;
568	if (phy->radio_rev == 8)
569	b43_calc_nrssi_offset(dev);
570
571	b43_phy_mask(dev, B43_PHY_G_CRS, mask: 0x7FFF);
572	b43_phy_mask(dev, offset: 0x0802, mask: 0xFFFC);
573	backup[7] = b43_read16(dev, offset: 0x03E2);
574	b43_write16(dev, offset: 0x03E2, value: b43_read16(dev, offset: 0x03E2) | 0x8000);
575	backup[0] = b43_radio_read16(dev, reg: 0x007A);
576	backup[1] = b43_radio_read16(dev, reg: 0x0052);
577	backup[2] = b43_radio_read16(dev, reg: 0x0043);
578	backup[3] = b43_phy_read(dev, reg: 0x0015);
579	backup[4] = b43_phy_read(dev, reg: 0x005A);
580	backup[5] = b43_phy_read(dev, reg: 0x0059);
581	backup[6] = b43_phy_read(dev, reg: 0x0058);
582	backup[8] = b43_read16(dev, offset: 0x03E6);
583	backup[9] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
584	if (phy->rev >= 3) {
585	backup[10] = b43_phy_read(dev, reg: 0x002E);
586	backup[11] = b43_phy_read(dev, reg: 0x002F);
587	backup[12] = b43_phy_read(dev, reg: 0x080F);
588	backup[13] = b43_phy_read(dev, B43_PHY_G_LO_CONTROL);
589	backup[14] = b43_phy_read(dev, reg: 0x0801);
590	backup[15] = b43_phy_read(dev, reg: 0x0060);
591	backup[16] = b43_phy_read(dev, reg: 0x0014);
592	backup[17] = b43_phy_read(dev, reg: 0x0478);
593	b43_phy_write(dev, reg: 0x002E, value: 0);
594	b43_phy_write(dev, B43_PHY_G_LO_CONTROL, value: 0);
595	switch (phy->rev) {
596	case 4:
597	case 6:
598	case 7:
599	b43_phy_set(dev, offset: 0x0478, set: 0x0100);
600	b43_phy_set(dev, offset: 0x0801, set: 0x0040);
601	break;
602	case 3:
603	case 5:
604	b43_phy_mask(dev, offset: 0x0801, mask: 0xFFBF);
605	break;
606	}
607	b43_phy_set(dev, offset: 0x0060, set: 0x0040);
608	b43_phy_set(dev, offset: 0x0014, set: 0x0200);
609	}
610	b43_radio_set(dev, offset: 0x007A, set: 0x0070);
611	b43_set_all_gains(dev, first: 0, second: 8, third: 0);
612	b43_radio_mask(dev, offset: 0x007A, mask: 0x00F7);
613	if (phy->rev >= 2) {
614	b43_phy_maskset(dev, offset: 0x0811, mask: 0xFFCF, set: 0x0030);
615	b43_phy_maskset(dev, offset: 0x0812, mask: 0xFFCF, set: 0x0010);
616	}
617	b43_radio_set(dev, offset: 0x007A, set: 0x0080);
618	udelay(20);
619
620	nrssi0 = (s16) ((b43_phy_read(dev, reg: 0x047F) >> 8) & 0x003F);
621	if (nrssi0 >= 0x0020)
622	nrssi0 -= 0x0040;
623
624	b43_radio_mask(dev, offset: 0x007A, mask: 0x007F);
625	if (phy->rev >= 2) {
626	b43_phy_maskset(dev, offset: 0x0003, mask: 0xFF9F, set: 0x0040);
627	}
628
629	b43_write16(dev, B43_MMIO_CHANNEL_EXT,
630	value: b43_read16(dev, B43_MMIO_CHANNEL_EXT)
631	| 0x2000);
632	b43_radio_set(dev, offset: 0x007A, set: 0x000F);
633	b43_phy_write(dev, reg: 0x0015, value: 0xF330);
634	if (phy->rev >= 2) {
635	b43_phy_maskset(dev, offset: 0x0812, mask: 0xFFCF, set: 0x0020);
636	b43_phy_maskset(dev, offset: 0x0811, mask: 0xFFCF, set: 0x0020);
637	}
638
639	b43_set_all_gains(dev, first: 3, second: 0, third: 1);
640	if (phy->radio_rev == 8) {
641	b43_radio_write16(dev, reg: 0x0043, value: 0x001F);
642	} else {
643	tmp = b43_radio_read16(dev, reg: 0x0052) & 0xFF0F;
644	b43_radio_write16(dev, reg: 0x0052, value: tmp | 0x0060);
645	tmp = b43_radio_read16(dev, reg: 0x0043) & 0xFFF0;
646	b43_radio_write16(dev, reg: 0x0043, value: tmp | 0x0009);
647	}
648	b43_phy_write(dev, reg: 0x005A, value: 0x0480);
649	b43_phy_write(dev, reg: 0x0059, value: 0x0810);
650	b43_phy_write(dev, reg: 0x0058, value: 0x000D);
651	udelay(20);
652	nrssi1 = (s16) ((b43_phy_read(dev, reg: 0x047F) >> 8) & 0x003F);
653	if (nrssi1 >= 0x0020)
654	nrssi1 -= 0x0040;
655	if (nrssi0 == nrssi1)
656	gphy->nrssislope = 0x00010000;
657	else
658	gphy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
659	if (nrssi0 >= -4) {
660	gphy->nrssi[0] = nrssi1;
661	gphy->nrssi[1] = nrssi0;
662	}
663	if (phy->rev >= 3) {
664	b43_phy_write(dev, reg: 0x002E, value: backup[10]);
665	b43_phy_write(dev, reg: 0x002F, value: backup[11]);
666	b43_phy_write(dev, reg: 0x080F, value: backup[12]);
667	b43_phy_write(dev, B43_PHY_G_LO_CONTROL, value: backup[13]);
668	}
669	if (phy->rev >= 2) {
670	b43_phy_mask(dev, offset: 0x0812, mask: 0xFFCF);
671	b43_phy_mask(dev, offset: 0x0811, mask: 0xFFCF);
672	}
673
674	b43_radio_write16(dev, reg: 0x007A, value: backup[0]);
675	b43_radio_write16(dev, reg: 0x0052, value: backup[1]);
676	b43_radio_write16(dev, reg: 0x0043, value: backup[2]);
677	b43_write16(dev, offset: 0x03E2, value: backup[7]);
678	b43_write16(dev, offset: 0x03E6, value: backup[8]);
679	b43_write16(dev, B43_MMIO_CHANNEL_EXT, value: backup[9]);
680	b43_phy_write(dev, reg: 0x0015, value: backup[3]);
681	b43_phy_write(dev, reg: 0x005A, value: backup[4]);
682	b43_phy_write(dev, reg: 0x0059, value: backup[5]);
683	b43_phy_write(dev, reg: 0x0058, value: backup[6]);
684	b43_synth_pu_workaround(dev, channel: phy->channel);
685	b43_phy_set(dev, offset: 0x0802, set: (0x0001 | 0x0002));
686	b43_set_original_gains(dev);
687	b43_phy_set(dev, B43_PHY_G_CRS, set: 0x8000);
688	if (phy->rev >= 3) {
689	b43_phy_write(dev, reg: 0x0801, value: backup[14]);
690	b43_phy_write(dev, reg: 0x0060, value: backup[15]);
691	b43_phy_write(dev, reg: 0x0014, value: backup[16]);
692	b43_phy_write(dev, reg: 0x0478, value: backup[17]);
693	}
694	b43_nrssi_mem_update(dev);
695	b43_calc_nrssi_threshold(dev);
696	}
697
698	static void b43_calc_nrssi_threshold(struct b43_wldev *dev)
699	{
700	struct b43_phy *phy = &dev->phy;
701	struct b43_phy_g *gphy = phy->g;
702	s32 a, b;
703	s16 tmp16;
704	u16 tmp_u16;
705
706	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
707
708	if (!phy->gmode ||
709	!(dev->dev->bus_sprom->boardflags_lo & B43_BFL_RSSI)) {
710	tmp16 = b43_nrssi_hw_read(dev, offset: 0x20);
711	if (tmp16 >= 0x20)
712	tmp16 -= 0x40;
713	if (tmp16 < 3) {
714	b43_phy_maskset(dev, offset: 0x048A, mask: 0xF000, set: 0x09EB);
715	} else {
716	b43_phy_maskset(dev, offset: 0x048A, mask: 0xF000, set: 0x0AED);
717	}
718	} else {
719	if (gphy->interfmode == B43_INTERFMODE_NONWLAN) {
720	a = 0xE;
721	b = 0xA;
722	} else if (!gphy->aci_wlan_automatic && gphy->aci_enable) {
723	a = 0x13;
724	b = 0x12;
725	} else {
726	a = 0xE;
727	b = 0x11;
728	}
729
730	a = a * (gphy->nrssi[1] - gphy->nrssi[0]);
731	a += (gphy->nrssi[0] << 6);
732	if (a < 32)
733	a += 31;
734	else
735	a += 32;
736	a = a >> 6;
737	a = clamp_val(a, -31, 31);
738
739	b = b * (gphy->nrssi[1] - gphy->nrssi[0]);
740	b += (gphy->nrssi[0] << 6);
741	if (b < 32)
742	b += 31;
743	else
744	b += 32;
745	b = b >> 6;
746	b = clamp_val(b, -31, 31);
747
748	tmp_u16 = b43_phy_read(dev, reg: 0x048A) & 0xF000;
749	tmp_u16 |= ((u32) b & 0x0000003F);
750	tmp_u16 |= (((u32) a & 0x0000003F) << 6);
751	b43_phy_write(dev, reg: 0x048A, value: tmp_u16);
752	}
753	}
754
755	/* Stack implementation to save/restore values from the
756	* interference mitigation code.
757	* It is save to restore values in random order.
758	*/
759	static void _stack_save(u32 *_stackptr, size_t *stackidx,
760	u8 id, u16 offset, u16 value)
761	{
762	u32 *stackptr = &(_stackptr[*stackidx]);
763
764	B43_WARN_ON(offset & 0xF000);
765	B43_WARN_ON(id & 0xF0);
766	*stackptr = offset;
767	*stackptr |= ((u32) id) << 12;
768	*stackptr |= ((u32) value) << 16;
769	(*stackidx)++;
770	B43_WARN_ON(*stackidx >= B43_INTERFSTACK_SIZE);
771	}
772
773	static u16 _stack_restore(u32 *stackptr, u8 id, u16 offset)
774	{
775	size_t i;
776
777	B43_WARN_ON(offset & 0xF000);
778	B43_WARN_ON(id & 0xF0);
779	for (i = 0; i < B43_INTERFSTACK_SIZE; i++, stackptr++) {
780	if ((*stackptr & 0x00000FFF) != offset)
781	continue;
782	if (((*stackptr & 0x0000F000) >> 12) != id)
783	continue;
784	return ((*stackptr & 0xFFFF0000) >> 16);
785	}
786	B43_WARN_ON(1);
787
788	return 0;
789	}
790
791	#define phy_stacksave(offset) \
792	do { \
793	_stack_save(stack, &stackidx, 0x1, (offset), \
794	b43_phy_read(dev, (offset))); \
795	} while (0)
796	#define phy_stackrestore(offset) \
797	do { \
798	b43_phy_write(dev, (offset), \
799	_stack_restore(stack, 0x1, \
800	(offset))); \
801	} while (0)
802	#define radio_stacksave(offset) \
803	do { \
804	_stack_save(stack, &stackidx, 0x2, (offset), \
805	b43_radio_read16(dev, (offset))); \
806	} while (0)
807	#define radio_stackrestore(offset) \
808	do { \
809	b43_radio_write16(dev, (offset), \
810	_stack_restore(stack, 0x2, \
811	(offset))); \
812	} while (0)
813	#define ofdmtab_stacksave(table, offset) \
814	do { \
815	_stack_save(stack, &stackidx, 0x3, (offset)|(table), \
816	b43_ofdmtab_read16(dev, (table), (offset))); \
817	} while (0)
818	#define ofdmtab_stackrestore(table, offset) \
819	do { \
820	b43_ofdmtab_write16(dev, (table), (offset), \
821	_stack_restore(stack, 0x3, \
822	(offset)|(table))); \
823	} while (0)
824
825	static void
826	b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode)
827	{
828	struct b43_phy *phy = &dev->phy;
829	struct b43_phy_g *gphy = phy->g;
830	u16 tmp, flipped;
831	size_t stackidx = 0;
832	u32 *stack = gphy->interfstack;
833
834	switch (mode) {
835	case B43_INTERFMODE_NONWLAN:
836	if (phy->rev != 1) {
837	b43_phy_set(dev, offset: 0x042B, set: 0x0800);
838	b43_phy_mask(dev, B43_PHY_G_CRS, mask: ~0x4000);
839	break;
840	}
841	radio_stacksave(0x0078);
842	tmp = (b43_radio_read16(dev, reg: 0x0078) & 0x001E);
843	B43_WARN_ON(tmp > 15);
844	flipped = bitrev4(tmp);
845	if (flipped < 10 && flipped >= 8)
846	flipped = 7;
847	else if (flipped >= 10)
848	flipped -= 3;
849	flipped = (bitrev4(flipped) << 1) | 0x0020;
850	b43_radio_write16(dev, reg: 0x0078, value: flipped);
851
852	b43_calc_nrssi_threshold(dev);
853
854	phy_stacksave(0x0406);
855	b43_phy_write(dev, reg: 0x0406, value: 0x7E28);
856
857	b43_phy_set(dev, offset: 0x042B, set: 0x0800);
858	b43_phy_set(dev, B43_PHY_RADIO_BITFIELD, set: 0x1000);
859
860	phy_stacksave(0x04A0);
861	b43_phy_maskset(dev, offset: 0x04A0, mask: 0xC0C0, set: 0x0008);
862	phy_stacksave(0x04A1);
863	b43_phy_maskset(dev, offset: 0x04A1, mask: 0xC0C0, set: 0x0605);
864	phy_stacksave(0x04A2);
865	b43_phy_maskset(dev, offset: 0x04A2, mask: 0xC0C0, set: 0x0204);
866	phy_stacksave(0x04A8);
867	b43_phy_maskset(dev, offset: 0x04A8, mask: 0xC0C0, set: 0x0803);
868	phy_stacksave(0x04AB);
869	b43_phy_maskset(dev, offset: 0x04AB, mask: 0xC0C0, set: 0x0605);
870
871	phy_stacksave(0x04A7);
872	b43_phy_write(dev, reg: 0x04A7, value: 0x0002);
873	phy_stacksave(0x04A3);
874	b43_phy_write(dev, reg: 0x04A3, value: 0x287A);
875	phy_stacksave(0x04A9);
876	b43_phy_write(dev, reg: 0x04A9, value: 0x2027);
877	phy_stacksave(0x0493);
878	b43_phy_write(dev, reg: 0x0493, value: 0x32F5);
879	phy_stacksave(0x04AA);
880	b43_phy_write(dev, reg: 0x04AA, value: 0x2027);
881	phy_stacksave(0x04AC);
882	b43_phy_write(dev, reg: 0x04AC, value: 0x32F5);
883	break;
884	case B43_INTERFMODE_MANUALWLAN:
885	if (b43_phy_read(dev, reg: 0x0033) & 0x0800)
886	break;
887
888	gphy->aci_enable = true;
889
890	phy_stacksave(B43_PHY_RADIO_BITFIELD);
891	phy_stacksave(B43_PHY_G_CRS);
892	if (phy->rev < 2) {
893	phy_stacksave(0x0406);
894	} else {
895	phy_stacksave(0x04C0);
896	phy_stacksave(0x04C1);
897	}
898	phy_stacksave(0x0033);
899	phy_stacksave(0x04A7);
900	phy_stacksave(0x04A3);
901	phy_stacksave(0x04A9);
902	phy_stacksave(0x04AA);
903	phy_stacksave(0x04AC);
904	phy_stacksave(0x0493);
905	phy_stacksave(0x04A1);
906	phy_stacksave(0x04A0);
907	phy_stacksave(0x04A2);
908	phy_stacksave(0x048A);
909	phy_stacksave(0x04A8);
910	phy_stacksave(0x04AB);
911	if (phy->rev == 2) {
912	phy_stacksave(0x04AD);
913	phy_stacksave(0x04AE);
914	} else if (phy->rev >= 3) {
915	phy_stacksave(0x04AD);
916	phy_stacksave(0x0415);
917	phy_stacksave(0x0416);
918	phy_stacksave(0x0417);
919	ofdmtab_stacksave(0x1A00, 0x2);
920	ofdmtab_stacksave(0x1A00, 0x3);
921	}
922	phy_stacksave(0x042B);
923	phy_stacksave(0x048C);
924
925	b43_phy_mask(dev, B43_PHY_RADIO_BITFIELD, mask: ~0x1000);
926	b43_phy_maskset(dev, B43_PHY_G_CRS, mask: 0xFFFC, set: 0x0002);
927
928	b43_phy_write(dev, reg: 0x0033, value: 0x0800);
929	b43_phy_write(dev, reg: 0x04A3, value: 0x2027);
930	b43_phy_write(dev, reg: 0x04A9, value: 0x1CA8);
931	b43_phy_write(dev, reg: 0x0493, value: 0x287A);
932	b43_phy_write(dev, reg: 0x04AA, value: 0x1CA8);
933	b43_phy_write(dev, reg: 0x04AC, value: 0x287A);
934
935	b43_phy_maskset(dev, offset: 0x04A0, mask: 0xFFC0, set: 0x001A);
936	b43_phy_write(dev, reg: 0x04A7, value: 0x000D);
937
938	if (phy->rev < 2) {
939	b43_phy_write(dev, reg: 0x0406, value: 0xFF0D);
940	} else if (phy->rev == 2) {
941	b43_phy_write(dev, reg: 0x04C0, value: 0xFFFF);
942	b43_phy_write(dev, reg: 0x04C1, value: 0x00A9);
943	} else {
944	b43_phy_write(dev, reg: 0x04C0, value: 0x00C1);
945	b43_phy_write(dev, reg: 0x04C1, value: 0x0059);
946	}
947
948	b43_phy_maskset(dev, offset: 0x04A1, mask: 0xC0FF, set: 0x1800);
949	b43_phy_maskset(dev, offset: 0x04A1, mask: 0xFFC0, set: 0x0015);
950	b43_phy_maskset(dev, offset: 0x04A8, mask: 0xCFFF, set: 0x1000);
951	b43_phy_maskset(dev, offset: 0x04A8, mask: 0xF0FF, set: 0x0A00);
952	b43_phy_maskset(dev, offset: 0x04AB, mask: 0xCFFF, set: 0x1000);
953	b43_phy_maskset(dev, offset: 0x04AB, mask: 0xF0FF, set: 0x0800);
954	b43_phy_maskset(dev, offset: 0x04AB, mask: 0xFFCF, set: 0x0010);
955	b43_phy_maskset(dev, offset: 0x04AB, mask: 0xFFF0, set: 0x0005);
956	b43_phy_maskset(dev, offset: 0x04A8, mask: 0xFFCF, set: 0x0010);
957	b43_phy_maskset(dev, offset: 0x04A8, mask: 0xFFF0, set: 0x0006);
958	b43_phy_maskset(dev, offset: 0x04A2, mask: 0xF0FF, set: 0x0800);
959	b43_phy_maskset(dev, offset: 0x04A0, mask: 0xF0FF, set: 0x0500);
960	b43_phy_maskset(dev, offset: 0x04A2, mask: 0xFFF0, set: 0x000B);
961
962	if (phy->rev >= 3) {
963	b43_phy_mask(dev, offset: 0x048A, mask: 0x7FFF);
964	b43_phy_maskset(dev, offset: 0x0415, mask: 0x8000, set: 0x36D8);
965	b43_phy_maskset(dev, offset: 0x0416, mask: 0x8000, set: 0x36D8);
966	b43_phy_maskset(dev, offset: 0x0417, mask: 0xFE00, set: 0x016D);
967	} else {
968	b43_phy_set(dev, offset: 0x048A, set: 0x1000);
969	b43_phy_maskset(dev, offset: 0x048A, mask: 0x9FFF, set: 0x2000);
970	b43_hf_write(dev, value: b43_hf_read(dev) | B43_HF_ACIW);
971	}
972	if (phy->rev >= 2) {
973	b43_phy_set(dev, offset: 0x042B, set: 0x0800);
974	}
975	b43_phy_maskset(dev, offset: 0x048C, mask: 0xF0FF, set: 0x0200);
976	if (phy->rev == 2) {
977	b43_phy_maskset(dev, offset: 0x04AE, mask: 0xFF00, set: 0x007F);
978	b43_phy_maskset(dev, offset: 0x04AD, mask: 0x00FF, set: 0x1300);
979	} else if (phy->rev >= 6) {
980	b43_ofdmtab_write16(dev, table: 0x1A00, offset: 0x3, value: 0x007F);
981	b43_ofdmtab_write16(dev, table: 0x1A00, offset: 0x2, value: 0x007F);
982	b43_phy_mask(dev, offset: 0x04AD, mask: 0x00FF);
983	}
984	b43_calc_nrssi_slope(dev);
985	break;
986	default:
987	B43_WARN_ON(1);
988	}
989	}
990
991	static void
992	b43_radio_interference_mitigation_disable(struct b43_wldev *dev, int mode)
993	{
994	struct b43_phy *phy = &dev->phy;
995	struct b43_phy_g *gphy = phy->g;
996	u32 *stack = gphy->interfstack;
997
998	switch (mode) {
999	case B43_INTERFMODE_NONWLAN:
1000	if (phy->rev != 1) {
1001	b43_phy_mask(dev, offset: 0x042B, mask: ~0x0800);
1002	b43_phy_set(dev, B43_PHY_G_CRS, set: 0x4000);
1003	break;
1004	}
1005	radio_stackrestore(0x0078);
1006	b43_calc_nrssi_threshold(dev);
1007	phy_stackrestore(0x0406);
1008	b43_phy_mask(dev, offset: 0x042B, mask: ~0x0800);
1009	if (!dev->bad_frames_preempt) {
1010	b43_phy_mask(dev, B43_PHY_RADIO_BITFIELD, mask: ~(1 << 11));
1011	}
1012	b43_phy_set(dev, B43_PHY_G_CRS, set: 0x4000);
1013	phy_stackrestore(0x04A0);
1014	phy_stackrestore(0x04A1);
1015	phy_stackrestore(0x04A2);
1016	phy_stackrestore(0x04A8);
1017	phy_stackrestore(0x04AB);
1018	phy_stackrestore(0x04A7);
1019	phy_stackrestore(0x04A3);
1020	phy_stackrestore(0x04A9);
1021	phy_stackrestore(0x0493);
1022	phy_stackrestore(0x04AA);
1023	phy_stackrestore(0x04AC);
1024	break;
1025	case B43_INTERFMODE_MANUALWLAN:
1026	if (!(b43_phy_read(dev, reg: 0x0033) & 0x0800))
1027	break;
1028
1029	gphy->aci_enable = false;
1030
1031	phy_stackrestore(B43_PHY_RADIO_BITFIELD);
1032	phy_stackrestore(B43_PHY_G_CRS);
1033	phy_stackrestore(0x0033);
1034	phy_stackrestore(0x04A3);
1035	phy_stackrestore(0x04A9);
1036	phy_stackrestore(0x0493);
1037	phy_stackrestore(0x04AA);
1038	phy_stackrestore(0x04AC);
1039	phy_stackrestore(0x04A0);
1040	phy_stackrestore(0x04A7);
1041	if (phy->rev >= 2) {
1042	phy_stackrestore(0x04C0);
1043	phy_stackrestore(0x04C1);
1044	} else
1045	phy_stackrestore(0x0406);
1046	phy_stackrestore(0x04A1);
1047	phy_stackrestore(0x04AB);
1048	phy_stackrestore(0x04A8);
1049	if (phy->rev == 2) {
1050	phy_stackrestore(0x04AD);
1051	phy_stackrestore(0x04AE);
1052	} else if (phy->rev >= 3) {
1053	phy_stackrestore(0x04AD);
1054	phy_stackrestore(0x0415);
1055	phy_stackrestore(0x0416);
1056	phy_stackrestore(0x0417);
1057	ofdmtab_stackrestore(0x1A00, 0x2);
1058	ofdmtab_stackrestore(0x1A00, 0x3);
1059	}
1060	phy_stackrestore(0x04A2);
1061	phy_stackrestore(0x048A);
1062	phy_stackrestore(0x042B);
1063	phy_stackrestore(0x048C);
1064	b43_hf_write(dev, value: b43_hf_read(dev) & ~B43_HF_ACIW);
1065	b43_calc_nrssi_slope(dev);
1066	break;
1067	default:
1068	B43_WARN_ON(1);
1069	}
1070	}
1071
1072	#undef phy_stacksave
1073	#undef phy_stackrestore
1074	#undef radio_stacksave
1075	#undef radio_stackrestore
1076	#undef ofdmtab_stacksave
1077	#undef ofdmtab_stackrestore
1078
1079	static u16 b43_radio_core_calibration_value(struct b43_wldev *dev)
1080	{
1081	u16 reg, index, ret;
1082
1083	static const u8 rcc_table[] = {
1084	0x02, 0x03, 0x01, 0x0F,
1085	0x06, 0x07, 0x05, 0x0F,
1086	0x0A, 0x0B, 0x09, 0x0F,
1087	0x0E, 0x0F, 0x0D, 0x0F,
1088	};
1089
1090	reg = b43_radio_read16(dev, reg: 0x60);
1091	index = (reg & 0x001E) >> 1;
1092	ret = rcc_table[index] << 1;
1093	ret |= (reg & 0x0001);
1094	ret |= 0x0020;
1095
1096	return ret;
1097	}
1098
1099	#define LPD(L, P, D) (((L) << 2) | ((P) << 1) | ((D) << 0))
1100	static u16 radio2050_rfover_val(struct b43_wldev *dev,
1101	u16 phy_register, unsigned int lpd)
1102	{
1103	struct b43_phy *phy = &dev->phy;
1104	struct b43_phy_g *gphy = phy->g;
1105	struct ssb_sprom *sprom = dev->dev->bus_sprom;
1106
1107	if (!phy->gmode)
1108	return 0;
1109
1110	if (has_loopback_gain(phy)) {
1111	int max_lb_gain = gphy->max_lb_gain;
1112	u16 extlna;
1113	u16 i;
1114
1115	if (phy->radio_rev == 8)
1116	max_lb_gain += 0x3E;
1117	else
1118	max_lb_gain += 0x26;
1119	if (max_lb_gain >= 0x46) {
1120	extlna = 0x3000;
1121	max_lb_gain -= 0x46;
1122	} else if (max_lb_gain >= 0x3A) {
1123	extlna = 0x1000;
1124	max_lb_gain -= 0x3A;
1125	} else if (max_lb_gain >= 0x2E) {
1126	extlna = 0x2000;
1127	max_lb_gain -= 0x2E;
1128	} else {
1129	extlna = 0;
1130	max_lb_gain -= 0x10;
1131	}
1132
1133	for (i = 0; i < 16; i++) {
1134	max_lb_gain -= (i * 6);
1135	if (max_lb_gain < 6)
1136	break;
1137	}
1138
1139	if ((phy->rev < 7) ||
1140	!(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
1141	if (phy_register == B43_PHY_RFOVER) {
1142	return 0x1B3;
1143	} else if (phy_register == B43_PHY_RFOVERVAL) {
1144	extlna |= (i << 8);
1145	switch (lpd) {
1146	case LPD(0, 1, 1):
1147	return 0x0F92;
1148	case LPD(0, 0, 1):
1149	case LPD(1, 0, 1):
1150	return (0x0092 | extlna);
1151	case LPD(1, 0, 0):
1152	return (0x0093 | extlna);
1153	}
1154	B43_WARN_ON(1);
1155	}
1156	B43_WARN_ON(1);
1157	} else {
1158	if (phy_register == B43_PHY_RFOVER) {
1159	return 0x9B3;
1160	} else if (phy_register == B43_PHY_RFOVERVAL) {
1161	if (extlna)
1162	extlna |= 0x8000;
1163	extlna |= (i << 8);
1164	switch (lpd) {
1165	case LPD(0, 1, 1):
1166	return 0x8F92;
1167	case LPD(0, 0, 1):
1168	return (0x8092 | extlna);
1169	case LPD(1, 0, 1):
1170	return (0x2092 | extlna);
1171	case LPD(1, 0, 0):
1172	return (0x2093 | extlna);
1173	}
1174	B43_WARN_ON(1);
1175	}
1176	B43_WARN_ON(1);
1177	}
1178	} else {
1179	if ((phy->rev < 7) ||
1180	!(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
1181	if (phy_register == B43_PHY_RFOVER) {
1182	return 0x1B3;
1183	} else if (phy_register == B43_PHY_RFOVERVAL) {
1184	switch (lpd) {
1185	case LPD(0, 1, 1):
1186	return 0x0FB2;
1187	case LPD(0, 0, 1):
1188	return 0x00B2;
1189	case LPD(1, 0, 1):
1190	return 0x30B2;
1191	case LPD(1, 0, 0):
1192	return 0x30B3;
1193	}
1194	B43_WARN_ON(1);
1195	}
1196	B43_WARN_ON(1);
1197	} else {
1198	if (phy_register == B43_PHY_RFOVER) {
1199	return 0x9B3;
1200	} else if (phy_register == B43_PHY_RFOVERVAL) {
1201	switch (lpd) {
1202	case LPD(0, 1, 1):
1203	return 0x8FB2;
1204	case LPD(0, 0, 1):
1205	return 0x80B2;
1206	case LPD(1, 0, 1):
1207	return 0x20B2;
1208	case LPD(1, 0, 0):
1209	return 0x20B3;
1210	}
1211	B43_WARN_ON(1);
1212	}
1213	B43_WARN_ON(1);
1214	}
1215	}
1216	return 0;
1217	}
1218
1219	struct init2050_saved_values {
1220	/* Core registers */
1221	u16 reg_3EC;
1222	u16 reg_3E6;
1223	u16 reg_3F4;
1224	/* Radio registers */
1225	u16 radio_43;
1226	u16 radio_51;
1227	u16 radio_52;
1228	/* PHY registers */
1229	u16 phy_pgactl;
1230	u16 phy_cck_5A;
1231	u16 phy_cck_59;
1232	u16 phy_cck_58;
1233	u16 phy_cck_30;
1234	u16 phy_rfover;
1235	u16 phy_rfoverval;
1236	u16 phy_analogover;
1237	u16 phy_analogoverval;
1238	u16 phy_crs0;
1239	u16 phy_classctl;
1240	u16 phy_lo_mask;
1241	u16 phy_lo_ctl;
1242	u16 phy_syncctl;
1243	};
1244
1245	static u16 b43_radio_init2050(struct b43_wldev *dev)
1246	{
1247	struct b43_phy *phy = &dev->phy;
1248	struct init2050_saved_values sav;
1249	u16 rcc;
1250	u16 radio78;
1251	u16 ret;
1252	u16 i, j;
1253	u32 tmp1 = 0, tmp2 = 0;
1254
1255	memset(&sav, 0, sizeof(sav)); /* get rid of "may be used uninitialized..." */
1256
1257	sav.radio_43 = b43_radio_read16(dev, reg: 0x43);
1258	sav.radio_51 = b43_radio_read16(dev, reg: 0x51);
1259	sav.radio_52 = b43_radio_read16(dev, reg: 0x52);
1260	sav.phy_pgactl = b43_phy_read(dev, B43_PHY_PGACTL);
1261	sav.phy_cck_5A = b43_phy_read(dev, B43_PHY_CCK(0x5A));
1262	sav.phy_cck_59 = b43_phy_read(dev, B43_PHY_CCK(0x59));
1263	sav.phy_cck_58 = b43_phy_read(dev, B43_PHY_CCK(0x58));
1264
1265	if (phy->type == B43_PHYTYPE_B) {
1266	sav.phy_cck_30 = b43_phy_read(dev, B43_PHY_CCK(0x30));
1267	sav.reg_3EC = b43_read16(dev, offset: 0x3EC);
1268
1269	b43_phy_write(dev, B43_PHY_CCK(0x30), value: 0xFF);
1270	b43_write16(dev, offset: 0x3EC, value: 0x3F3F);
1271	} else if (phy->gmode || phy->rev >= 2) {
1272	sav.phy_rfover = b43_phy_read(dev, B43_PHY_RFOVER);
1273	sav.phy_rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
1274	sav.phy_analogover = b43_phy_read(dev, B43_PHY_ANALOGOVER);
1275	sav.phy_analogoverval =
1276	b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
1277	sav.phy_crs0 = b43_phy_read(dev, B43_PHY_CRS0);
1278	sav.phy_classctl = b43_phy_read(dev, B43_PHY_CLASSCTL);
1279
1280	b43_phy_set(dev, B43_PHY_ANALOGOVER, set: 0x0003);
1281	b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, mask: 0xFFFC);
1282	b43_phy_mask(dev, B43_PHY_CRS0, mask: 0x7FFF);
1283	b43_phy_mask(dev, B43_PHY_CLASSCTL, mask: 0xFFFC);
1284	if (has_loopback_gain(phy)) {
1285	sav.phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
1286	sav.phy_lo_ctl = b43_phy_read(dev, B43_PHY_LO_CTL);
1287
1288	if (phy->rev >= 3)
1289	b43_phy_write(dev, B43_PHY_LO_MASK, value: 0xC020);
1290	else
1291	b43_phy_write(dev, B43_PHY_LO_MASK, value: 0x8020);
1292	b43_phy_write(dev, B43_PHY_LO_CTL, value: 0);
1293	}
1294
1295	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1296	value: radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
1297	LPD(0, 1, 1)));
1298	b43_phy_write(dev, B43_PHY_RFOVER,
1299	value: radio2050_rfover_val(dev, B43_PHY_RFOVER, lpd: 0));
1300	}
1301	b43_write16(dev, offset: 0x3E2, value: b43_read16(dev, offset: 0x3E2) | 0x8000);
1302
1303	sav.phy_syncctl = b43_phy_read(dev, B43_PHY_SYNCCTL);
1304	b43_phy_mask(dev, B43_PHY_SYNCCTL, mask: 0xFF7F);
1305	sav.reg_3E6 = b43_read16(dev, offset: 0x3E6);
1306	sav.reg_3F4 = b43_read16(dev, offset: 0x3F4);
1307
1308	if (phy->analog == 0) {
1309	b43_write16(dev, offset: 0x03E6, value: 0x0122);
1310	} else {
1311	if (phy->analog >= 2) {
1312	b43_phy_maskset(dev, B43_PHY_CCK(0x03), mask: 0xFFBF, set: 0x40);
1313	}
1314	b43_write16(dev, B43_MMIO_CHANNEL_EXT,
1315	value: (b43_read16(dev, B43_MMIO_CHANNEL_EXT) | 0x2000));
1316	}
1317
1318	rcc = b43_radio_core_calibration_value(dev);
1319
1320	if (phy->type == B43_PHYTYPE_B)
1321	b43_radio_write16(dev, reg: 0x78, value: 0x26);
1322	if (phy->gmode || phy->rev >= 2) {
1323	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1324	value: radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
1325	LPD(0, 1, 1)));
1326	}
1327	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xBFAF);
1328	b43_phy_write(dev, B43_PHY_CCK(0x2B), value: 0x1403);
1329	if (phy->gmode || phy->rev >= 2) {
1330	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1331	value: radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
1332	LPD(0, 0, 1)));
1333	}
1334	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xBFA0);
1335	b43_radio_set(dev, offset: 0x51, set: 0x0004);
1336	if (phy->radio_rev == 8) {
1337	b43_radio_write16(dev, reg: 0x43, value: 0x1F);
1338	} else {
1339	b43_radio_write16(dev, reg: 0x52, value: 0);
1340	b43_radio_maskset(dev, offset: 0x43, mask: 0xFFF0, set: 0x0009);
1341	}
1342	b43_phy_write(dev, B43_PHY_CCK(0x58), value: 0);
1343
1344	for (i = 0; i < 16; i++) {
1345	b43_phy_write(dev, B43_PHY_CCK(0x5A), value: 0x0480);
1346	b43_phy_write(dev, B43_PHY_CCK(0x59), value: 0xC810);
1347	b43_phy_write(dev, B43_PHY_CCK(0x58), value: 0x000D);
1348	if (phy->gmode || phy->rev >= 2) {
1349	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1350	value: radio2050_rfover_val(dev,
1351	B43_PHY_RFOVERVAL,
1352	LPD(1, 0, 1)));
1353	}
1354	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xAFB0);
1355	udelay(10);
1356	if (phy->gmode || phy->rev >= 2) {
1357	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1358	value: radio2050_rfover_val(dev,
1359	B43_PHY_RFOVERVAL,
1360	LPD(1, 0, 1)));
1361	}
1362	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xEFB0);
1363	udelay(10);
1364	if (phy->gmode || phy->rev >= 2) {
1365	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1366	value: radio2050_rfover_val(dev,
1367	B43_PHY_RFOVERVAL,
1368	LPD(1, 0, 0)));
1369	}
1370	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xFFF0);
1371	udelay(20);
1372	tmp1 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
1373	b43_phy_write(dev, B43_PHY_CCK(0x58), value: 0);
1374	if (phy->gmode || phy->rev >= 2) {
1375	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1376	value: radio2050_rfover_val(dev,
1377	B43_PHY_RFOVERVAL,
1378	LPD(1, 0, 1)));
1379	}
1380	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xAFB0);
1381	}
1382	udelay(10);
1383
1384	b43_phy_write(dev, B43_PHY_CCK(0x58), value: 0);
1385	tmp1++;
1386	tmp1 >>= 9;
1387
1388	for (i = 0; i < 16; i++) {
1389	radio78 = (bitrev4(i) << 1) | 0x0020;
1390	b43_radio_write16(dev, reg: 0x78, value: radio78);
1391	udelay(10);
1392	for (j = 0; j < 16; j++) {
1393	b43_phy_write(dev, B43_PHY_CCK(0x5A), value: 0x0D80);
1394	b43_phy_write(dev, B43_PHY_CCK(0x59), value: 0xC810);
1395	b43_phy_write(dev, B43_PHY_CCK(0x58), value: 0x000D);
1396	if (phy->gmode || phy->rev >= 2) {
1397	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1398	value: radio2050_rfover_val(dev,
1399	B43_PHY_RFOVERVAL,
1400	LPD(1, 0,
1401	1)));
1402	}
1403	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xAFB0);
1404	udelay(10);
1405	if (phy->gmode || phy->rev >= 2) {
1406	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1407	value: radio2050_rfover_val(dev,
1408	B43_PHY_RFOVERVAL,
1409	LPD(1, 0,
1410	1)));
1411	}
1412	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xEFB0);
1413	udelay(10);
1414	if (phy->gmode || phy->rev >= 2) {
1415	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1416	value: radio2050_rfover_val(dev,
1417	B43_PHY_RFOVERVAL,
1418	LPD(1, 0,
1419	0)));
1420	}
1421	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xFFF0);
1422	udelay(10);
1423	tmp2 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
1424	b43_phy_write(dev, B43_PHY_CCK(0x58), value: 0);
1425	if (phy->gmode || phy->rev >= 2) {
1426	b43_phy_write(dev, B43_PHY_RFOVERVAL,
1427	value: radio2050_rfover_val(dev,
1428	B43_PHY_RFOVERVAL,
1429	LPD(1, 0,
1430	1)));
1431	}
1432	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xAFB0);
1433	}
1434	tmp2++;
1435	tmp2 >>= 8;
1436	if (tmp1 < tmp2)
1437	break;
1438	}
1439
1440	/* Restore the registers */
1441	b43_phy_write(dev, B43_PHY_PGACTL, value: sav.phy_pgactl);
1442	b43_radio_write16(dev, reg: 0x51, value: sav.radio_51);
1443	b43_radio_write16(dev, reg: 0x52, value: sav.radio_52);
1444	b43_radio_write16(dev, reg: 0x43, value: sav.radio_43);
1445	b43_phy_write(dev, B43_PHY_CCK(0x5A), value: sav.phy_cck_5A);
1446	b43_phy_write(dev, B43_PHY_CCK(0x59), value: sav.phy_cck_59);
1447	b43_phy_write(dev, B43_PHY_CCK(0x58), value: sav.phy_cck_58);
1448	b43_write16(dev, offset: 0x3E6, value: sav.reg_3E6);
1449	if (phy->analog != 0)
1450	b43_write16(dev, offset: 0x3F4, value: sav.reg_3F4);
1451	b43_phy_write(dev, B43_PHY_SYNCCTL, value: sav.phy_syncctl);
1452	b43_synth_pu_workaround(dev, channel: phy->channel);
1453	if (phy->type == B43_PHYTYPE_B) {
1454	b43_phy_write(dev, B43_PHY_CCK(0x30), value: sav.phy_cck_30);
1455	b43_write16(dev, offset: 0x3EC, value: sav.reg_3EC);
1456	} else if (phy->gmode) {
1457	b43_write16(dev, B43_MMIO_PHY_RADIO,
1458	value: b43_read16(dev, B43_MMIO_PHY_RADIO)
1459	& 0x7FFF);
1460	b43_phy_write(dev, B43_PHY_RFOVER, value: sav.phy_rfover);
1461	b43_phy_write(dev, B43_PHY_RFOVERVAL, value: sav.phy_rfoverval);
1462	b43_phy_write(dev, B43_PHY_ANALOGOVER, value: sav.phy_analogover);
1463	b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
1464	value: sav.phy_analogoverval);
1465	b43_phy_write(dev, B43_PHY_CRS0, value: sav.phy_crs0);
1466	b43_phy_write(dev, B43_PHY_CLASSCTL, value: sav.phy_classctl);
1467	if (has_loopback_gain(phy)) {
1468	b43_phy_write(dev, B43_PHY_LO_MASK, value: sav.phy_lo_mask);
1469	b43_phy_write(dev, B43_PHY_LO_CTL, value: sav.phy_lo_ctl);
1470	}
1471	}
1472	if (i > 15)
1473	ret = radio78;
1474	else
1475	ret = rcc;
1476
1477	return ret;
1478	}
1479
1480	static void b43_phy_initb5(struct b43_wldev *dev)
1481	{
1482	struct b43_phy *phy = &dev->phy;
1483	struct b43_phy_g *gphy = phy->g;
1484	u16 offset, value;
1485	u8 old_channel;
1486
1487	if (phy->analog == 1) {
1488	b43_radio_set(dev, offset: 0x007A, set: 0x0050);
1489	}
1490	if ((dev->dev->board_vendor != SSB_BOARDVENDOR_BCM) &&
1491	(dev->dev->board_type != SSB_BOARD_BU4306)) {
1492	value = 0x2120;
1493	for (offset = 0x00A8; offset < 0x00C7; offset++) {
1494	b43_phy_write(dev, reg: offset, value);
1495	value += 0x202;
1496	}
1497	}
1498	b43_phy_maskset(dev, offset: 0x0035, mask: 0xF0FF, set: 0x0700);
1499	if (phy->radio_ver == 0x2050)
1500	b43_phy_write(dev, reg: 0x0038, value: 0x0667);
1501
1502	if (phy->gmode || phy->rev >= 2) {
1503	if (phy->radio_ver == 0x2050) {
1504	b43_radio_set(dev, offset: 0x007A, set: 0x0020);
1505	b43_radio_set(dev, offset: 0x0051, set: 0x0004);
1506	}
1507	b43_write16(dev, B43_MMIO_PHY_RADIO, value: 0x0000);
1508
1509	b43_phy_set(dev, offset: 0x0802, set: 0x0100);
1510	b43_phy_set(dev, offset: 0x042B, set: 0x2000);
1511
1512	b43_phy_write(dev, reg: 0x001C, value: 0x186A);
1513
1514	b43_phy_maskset(dev, offset: 0x0013, mask: 0x00FF, set: 0x1900);
1515	b43_phy_maskset(dev, offset: 0x0035, mask: 0xFFC0, set: 0x0064);
1516	b43_phy_maskset(dev, offset: 0x005D, mask: 0xFF80, set: 0x000A);
1517	}
1518
1519	if (dev->bad_frames_preempt) {
1520	b43_phy_set(dev, B43_PHY_RADIO_BITFIELD, set: (1 << 11));
1521	}
1522
1523	if (phy->analog == 1) {
1524	b43_phy_write(dev, reg: 0x0026, value: 0xCE00);
1525	b43_phy_write(dev, reg: 0x0021, value: 0x3763);
1526	b43_phy_write(dev, reg: 0x0022, value: 0x1BC3);
1527	b43_phy_write(dev, reg: 0x0023, value: 0x06F9);
1528	b43_phy_write(dev, reg: 0x0024, value: 0x037E);
1529	} else
1530	b43_phy_write(dev, reg: 0x0026, value: 0xCC00);
1531	b43_phy_write(dev, reg: 0x0030, value: 0x00C6);
1532	b43_write16(dev, offset: 0x03EC, value: 0x3F22);
1533
1534	if (phy->analog == 1)
1535	b43_phy_write(dev, reg: 0x0020, value: 0x3E1C);
1536	else
1537	b43_phy_write(dev, reg: 0x0020, value: 0x301C);
1538
1539	if (phy->analog == 0)
1540	b43_write16(dev, offset: 0x03E4, value: 0x3000);
1541
1542	old_channel = phy->channel;
1543	/* Force to channel 7, even if not supported. */
1544	b43_gphy_channel_switch(dev, channel: 7, synthetic_pu_workaround: 0);
1545
1546	if (phy->radio_ver != 0x2050) {
1547	b43_radio_write16(dev, reg: 0x0075, value: 0x0080);
1548	b43_radio_write16(dev, reg: 0x0079, value: 0x0081);
1549	}
1550
1551	b43_radio_write16(dev, reg: 0x0050, value: 0x0020);
1552	b43_radio_write16(dev, reg: 0x0050, value: 0x0023);
1553
1554	if (phy->radio_ver == 0x2050) {
1555	b43_radio_write16(dev, reg: 0x0050, value: 0x0020);
1556	b43_radio_write16(dev, reg: 0x005A, value: 0x0070);
1557	}
1558
1559	b43_radio_write16(dev, reg: 0x005B, value: 0x007B);
1560	b43_radio_write16(dev, reg: 0x005C, value: 0x00B0);
1561
1562	b43_radio_set(dev, offset: 0x007A, set: 0x0007);
1563
1564	b43_gphy_channel_switch(dev, channel: old_channel, synthetic_pu_workaround: 0);
1565
1566	b43_phy_write(dev, reg: 0x0014, value: 0x0080);
1567	b43_phy_write(dev, reg: 0x0032, value: 0x00CA);
1568	b43_phy_write(dev, reg: 0x002A, value: 0x88A3);
1569
1570	b43_set_txpower_g(dev, bbatt: &gphy->bbatt, rfatt: &gphy->rfatt, tx_control: gphy->tx_control);
1571
1572	if (phy->radio_ver == 0x2050)
1573	b43_radio_write16(dev, reg: 0x005D, value: 0x000D);
1574
1575	b43_write16(dev, offset: 0x03E4, value: (b43_read16(dev, offset: 0x03E4) & 0xFFC0) | 0x0004);
1576	}
1577
1578	/* https://bcm-v4.sipsolutions.net/802.11/PHY/Init/B6 */
1579	static void b43_phy_initb6(struct b43_wldev *dev)
1580	{
1581	struct b43_phy *phy = &dev->phy;
1582	struct b43_phy_g *gphy = phy->g;
1583	u16 offset, val;
1584	u8 old_channel;
1585
1586	b43_phy_write(dev, reg: 0x003E, value: 0x817A);
1587	b43_radio_write16(dev, reg: 0x007A,
1588	value: (b43_radio_read16(dev, reg: 0x007A) | 0x0058));
1589	if (phy->radio_rev == 4 || phy->radio_rev == 5) {
1590	b43_radio_write16(dev, reg: 0x51, value: 0x37);
1591	b43_radio_write16(dev, reg: 0x52, value: 0x70);
1592	b43_radio_write16(dev, reg: 0x53, value: 0xB3);
1593	b43_radio_write16(dev, reg: 0x54, value: 0x9B);
1594	b43_radio_write16(dev, reg: 0x5A, value: 0x88);
1595	b43_radio_write16(dev, reg: 0x5B, value: 0x88);
1596	b43_radio_write16(dev, reg: 0x5D, value: 0x88);
1597	b43_radio_write16(dev, reg: 0x5E, value: 0x88);
1598	b43_radio_write16(dev, reg: 0x7D, value: 0x88);
1599	b43_hf_write(dev, value: b43_hf_read(dev)
1600	| B43_HF_TSSIRPSMW);
1601	}
1602	B43_WARN_ON(phy->radio_rev == 6 || phy->radio_rev == 7); /* We had code for these revs here... */
1603	if (phy->radio_rev == 8) {
1604	b43_radio_write16(dev, reg: 0x51, value: 0);
1605	b43_radio_write16(dev, reg: 0x52, value: 0x40);
1606	b43_radio_write16(dev, reg: 0x53, value: 0xB7);
1607	b43_radio_write16(dev, reg: 0x54, value: 0x98);
1608	b43_radio_write16(dev, reg: 0x5A, value: 0x88);
1609	b43_radio_write16(dev, reg: 0x5B, value: 0x6B);
1610	b43_radio_write16(dev, reg: 0x5C, value: 0x0F);
1611	if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_ALTIQ) {
1612	b43_radio_write16(dev, reg: 0x5D, value: 0xFA);
1613	b43_radio_write16(dev, reg: 0x5E, value: 0xD8);
1614	} else {
1615	b43_radio_write16(dev, reg: 0x5D, value: 0xF5);
1616	b43_radio_write16(dev, reg: 0x5E, value: 0xB8);
1617	}
1618	b43_radio_write16(dev, reg: 0x0073, value: 0x0003);
1619	b43_radio_write16(dev, reg: 0x007D, value: 0x00A8);
1620	b43_radio_write16(dev, reg: 0x007C, value: 0x0001);
1621	b43_radio_write16(dev, reg: 0x007E, value: 0x0008);
1622	}
1623	val = 0x1E1F;
1624	for (offset = 0x0088; offset < 0x0098; offset++) {
1625	b43_phy_write(dev, reg: offset, value: val);
1626	val -= 0x0202;
1627	}
1628	val = 0x3E3F;
1629	for (offset = 0x0098; offset < 0x00A8; offset++) {
1630	b43_phy_write(dev, reg: offset, value: val);
1631	val -= 0x0202;
1632	}
1633	val = 0x2120;
1634	for (offset = 0x00A8; offset < 0x00C8; offset++) {
1635	b43_phy_write(dev, reg: offset, value: (val & 0x3F3F));
1636	val += 0x0202;
1637	}
1638	if (phy->type == B43_PHYTYPE_G) {
1639	b43_radio_set(dev, offset: 0x007A, set: 0x0020);
1640	b43_radio_set(dev, offset: 0x0051, set: 0x0004);
1641	b43_phy_set(dev, offset: 0x0802, set: 0x0100);
1642	b43_phy_set(dev, offset: 0x042B, set: 0x2000);
1643	b43_phy_write(dev, reg: 0x5B, value: 0);
1644	b43_phy_write(dev, reg: 0x5C, value: 0);
1645	}
1646
1647	old_channel = phy->channel;
1648	if (old_channel >= 8)
1649	b43_gphy_channel_switch(dev, channel: 1, synthetic_pu_workaround: 0);
1650	else
1651	b43_gphy_channel_switch(dev, channel: 13, synthetic_pu_workaround: 0);
1652
1653	b43_radio_write16(dev, reg: 0x0050, value: 0x0020);
1654	b43_radio_write16(dev, reg: 0x0050, value: 0x0023);
1655	udelay(40);
1656	if (phy->radio_rev < 6 || phy->radio_rev == 8) {
1657	b43_radio_write16(dev, reg: 0x7C, value: (b43_radio_read16(dev, reg: 0x7C)
1658	| 0x0002));
1659	b43_radio_write16(dev, reg: 0x50, value: 0x20);
1660	}
1661	if (phy->radio_rev <= 2) {
1662	b43_radio_write16(dev, reg: 0x50, value: 0x20);
1663	b43_radio_write16(dev, reg: 0x5A, value: 0x70);
1664	b43_radio_write16(dev, reg: 0x5B, value: 0x7B);
1665	b43_radio_write16(dev, reg: 0x5C, value: 0xB0);
1666	}
1667	b43_radio_maskset(dev, offset: 0x007A, mask: 0x00F8, set: 0x0007);
1668
1669	b43_gphy_channel_switch(dev, channel: old_channel, synthetic_pu_workaround: 0);
1670
1671	b43_phy_write(dev, reg: 0x0014, value: 0x0200);
1672	if (phy->radio_rev >= 6)
1673	b43_phy_write(dev, reg: 0x2A, value: 0x88C2);
1674	else
1675	b43_phy_write(dev, reg: 0x2A, value: 0x8AC0);
1676	b43_phy_write(dev, reg: 0x0038, value: 0x0668);
1677	b43_set_txpower_g(dev, bbatt: &gphy->bbatt, rfatt: &gphy->rfatt, tx_control: gphy->tx_control);
1678	if (phy->radio_rev == 4 || phy->radio_rev == 5)
1679	b43_phy_maskset(dev, offset: 0x5D, mask: 0xFF80, set: 0x0003);
1680	if (phy->radio_rev <= 2)
1681	b43_radio_write16(dev, reg: 0x005D, value: 0x000D);
1682
1683	if (phy->analog == 4) {
1684	b43_write16(dev, offset: 0x3E4, value: 9);
1685	b43_phy_mask(dev, offset: 0x61, mask: 0x0FFF);
1686	} else {
1687	b43_phy_maskset(dev, offset: 0x0002, mask: 0xFFC0, set: 0x0004);
1688	}
1689	if (phy->type == B43_PHYTYPE_B)
1690	B43_WARN_ON(1);
1691	else if (phy->type == B43_PHYTYPE_G)
1692	b43_write16(dev, offset: 0x03E6, value: 0x0);
1693	}
1694
1695	static void b43_calc_loopback_gain(struct b43_wldev *dev)
1696	{
1697	struct b43_phy *phy = &dev->phy;
1698	struct b43_phy_g *gphy = phy->g;
1699	u16 backup_phy[16] = { 0 };
1700	u16 backup_radio[3];
1701	u16 backup_bband;
1702	u16 i, j, loop_i_max;
1703	u16 trsw_rx;
1704	u16 loop1_outer_done, loop1_inner_done;
1705
1706	backup_phy[0] = b43_phy_read(dev, B43_PHY_CRS0);
1707	backup_phy[1] = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
1708	backup_phy[2] = b43_phy_read(dev, B43_PHY_RFOVER);
1709	backup_phy[3] = b43_phy_read(dev, B43_PHY_RFOVERVAL);
1710	if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */
1711	backup_phy[4] = b43_phy_read(dev, B43_PHY_ANALOGOVER);
1712	backup_phy[5] = b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
1713	}
1714	backup_phy[6] = b43_phy_read(dev, B43_PHY_CCK(0x5A));
1715	backup_phy[7] = b43_phy_read(dev, B43_PHY_CCK(0x59));
1716	backup_phy[8] = b43_phy_read(dev, B43_PHY_CCK(0x58));
1717	backup_phy[9] = b43_phy_read(dev, B43_PHY_CCK(0x0A));
1718	backup_phy[10] = b43_phy_read(dev, B43_PHY_CCK(0x03));
1719	backup_phy[11] = b43_phy_read(dev, B43_PHY_LO_MASK);
1720	backup_phy[12] = b43_phy_read(dev, B43_PHY_LO_CTL);
1721	backup_phy[13] = b43_phy_read(dev, B43_PHY_CCK(0x2B));
1722	backup_phy[14] = b43_phy_read(dev, B43_PHY_PGACTL);
1723	backup_phy[15] = b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
1724	backup_bband = gphy->bbatt.att;
1725	backup_radio[0] = b43_radio_read16(dev, reg: 0x52);
1726	backup_radio[1] = b43_radio_read16(dev, reg: 0x43);
1727	backup_radio[2] = b43_radio_read16(dev, reg: 0x7A);
1728
1729	b43_phy_mask(dev, B43_PHY_CRS0, mask: 0x3FFF);
1730	b43_phy_set(dev, B43_PHY_CCKBBANDCFG, set: 0x8000);
1731	b43_phy_set(dev, B43_PHY_RFOVER, set: 0x0002);
1732	b43_phy_mask(dev, B43_PHY_RFOVERVAL, mask: 0xFFFD);
1733	b43_phy_set(dev, B43_PHY_RFOVER, set: 0x0001);
1734	b43_phy_mask(dev, B43_PHY_RFOVERVAL, mask: 0xFFFE);
1735	if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */
1736	b43_phy_set(dev, B43_PHY_ANALOGOVER, set: 0x0001);
1737	b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, mask: 0xFFFE);
1738	b43_phy_set(dev, B43_PHY_ANALOGOVER, set: 0x0002);
1739	b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, mask: 0xFFFD);
1740	}
1741	b43_phy_set(dev, B43_PHY_RFOVER, set: 0x000C);
1742	b43_phy_set(dev, B43_PHY_RFOVERVAL, set: 0x000C);
1743	b43_phy_set(dev, B43_PHY_RFOVER, set: 0x0030);
1744	b43_phy_maskset(dev, B43_PHY_RFOVERVAL, mask: 0xFFCF, set: 0x10);
1745
1746	b43_phy_write(dev, B43_PHY_CCK(0x5A), value: 0x0780);
1747	b43_phy_write(dev, B43_PHY_CCK(0x59), value: 0xC810);
1748	b43_phy_write(dev, B43_PHY_CCK(0x58), value: 0x000D);
1749
1750	b43_phy_set(dev, B43_PHY_CCK(0x0A), set: 0x2000);
1751	if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */
1752	b43_phy_set(dev, B43_PHY_ANALOGOVER, set: 0x0004);
1753	b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, mask: 0xFFFB);
1754	}
1755	b43_phy_maskset(dev, B43_PHY_CCK(0x03), mask: 0xFF9F, set: 0x40);
1756
1757	if (phy->radio_rev == 8) {
1758	b43_radio_write16(dev, reg: 0x43, value: 0x000F);
1759	} else {
1760	b43_radio_write16(dev, reg: 0x52, value: 0);
1761	b43_radio_maskset(dev, offset: 0x43, mask: 0xFFF0, set: 0x9);
1762	}
1763	b43_gphy_set_baseband_attenuation(dev, baseband_attenuation: 11);
1764
1765	if (phy->rev >= 3)
1766	b43_phy_write(dev, B43_PHY_LO_MASK, value: 0xC020);
1767	else
1768	b43_phy_write(dev, B43_PHY_LO_MASK, value: 0x8020);
1769	b43_phy_write(dev, B43_PHY_LO_CTL, value: 0);
1770
1771	b43_phy_maskset(dev, B43_PHY_CCK(0x2B), mask: 0xFFC0, set: 0x01);
1772	b43_phy_maskset(dev, B43_PHY_CCK(0x2B), mask: 0xC0FF, set: 0x800);
1773
1774	b43_phy_set(dev, B43_PHY_RFOVER, set: 0x0100);
1775	b43_phy_mask(dev, B43_PHY_RFOVERVAL, mask: 0xCFFF);
1776
1777	if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_EXTLNA) {
1778	if (phy->rev >= 7) {
1779	b43_phy_set(dev, B43_PHY_RFOVER, set: 0x0800);
1780	b43_phy_set(dev, B43_PHY_RFOVERVAL, set: 0x8000);
1781	}
1782	}
1783	b43_radio_mask(dev, offset: 0x7A, mask: 0x00F7);
1784
1785	j = 0;
1786	loop_i_max = (phy->radio_rev == 8) ? 15 : 9;
1787	for (i = 0; i < loop_i_max; i++) {
1788	for (j = 0; j < 16; j++) {
1789	b43_radio_write16(dev, reg: 0x43, value: i);
1790	b43_phy_maskset(dev, B43_PHY_RFOVERVAL, mask: 0xF0FF, set: (j << 8));
1791	b43_phy_maskset(dev, B43_PHY_PGACTL, mask: 0x0FFF, set: 0xA000);
1792	b43_phy_set(dev, B43_PHY_PGACTL, set: 0xF000);
1793	udelay(20);
1794	if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
1795	goto exit_loop1;
1796	}
1797	}
1798	exit_loop1:
1799	loop1_outer_done = i;
1800	loop1_inner_done = j;
1801	if (j >= 8) {
1802	b43_phy_set(dev, B43_PHY_RFOVERVAL, set: 0x30);
1803	trsw_rx = 0x1B;
1804	for (j = j - 8; j < 16; j++) {
1805	b43_phy_maskset(dev, B43_PHY_RFOVERVAL, mask: 0xF0FF, set: (j << 8));
1806	b43_phy_maskset(dev, B43_PHY_PGACTL, mask: 0x0FFF, set: 0xA000);
1807	b43_phy_set(dev, B43_PHY_PGACTL, set: 0xF000);
1808	udelay(20);
1809	trsw_rx -= 3;
1810	if (b43_phy_read(dev, B43_PHY_LO_LEAKAGE) >= 0xDFC)
1811	goto exit_loop2;
1812	}
1813	} else
1814	trsw_rx = 0x18;
1815	exit_loop2:
1816
1817	if (phy->rev != 1) { /* Not in specs, but needed to prevent PPC machine check */
1818	b43_phy_write(dev, B43_PHY_ANALOGOVER, value: backup_phy[4]);
1819	b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, value: backup_phy[5]);
1820	}
1821	b43_phy_write(dev, B43_PHY_CCK(0x5A), value: backup_phy[6]);
1822	b43_phy_write(dev, B43_PHY_CCK(0x59), value: backup_phy[7]);
1823	b43_phy_write(dev, B43_PHY_CCK(0x58), value: backup_phy[8]);
1824	b43_phy_write(dev, B43_PHY_CCK(0x0A), value: backup_phy[9]);
1825	b43_phy_write(dev, B43_PHY_CCK(0x03), value: backup_phy[10]);
1826	b43_phy_write(dev, B43_PHY_LO_MASK, value: backup_phy[11]);
1827	b43_phy_write(dev, B43_PHY_LO_CTL, value: backup_phy[12]);
1828	b43_phy_write(dev, B43_PHY_CCK(0x2B), value: backup_phy[13]);
1829	b43_phy_write(dev, B43_PHY_PGACTL, value: backup_phy[14]);
1830
1831	b43_gphy_set_baseband_attenuation(dev, baseband_attenuation: backup_bband);
1832
1833	b43_radio_write16(dev, reg: 0x52, value: backup_radio[0]);
1834	b43_radio_write16(dev, reg: 0x43, value: backup_radio[1]);
1835	b43_radio_write16(dev, reg: 0x7A, value: backup_radio[2]);
1836
1837	b43_phy_write(dev, B43_PHY_RFOVER, value: backup_phy[2] | 0x0003);
1838	udelay(10);
1839	b43_phy_write(dev, B43_PHY_RFOVER, value: backup_phy[2]);
1840	b43_phy_write(dev, B43_PHY_RFOVERVAL, value: backup_phy[3]);
1841	b43_phy_write(dev, B43_PHY_CRS0, value: backup_phy[0]);
1842	b43_phy_write(dev, B43_PHY_CCKBBANDCFG, value: backup_phy[1]);
1843
1844	gphy->max_lb_gain =
1845	((loop1_inner_done * 6) - (loop1_outer_done * 4)) - 11;
1846	gphy->trsw_rx_gain = trsw_rx * 2;
1847	}
1848
1849	static void b43_hardware_pctl_early_init(struct b43_wldev *dev)
1850	{
1851	struct b43_phy *phy = &dev->phy;
1852
1853	if (!b43_has_hardware_pctl(dev)) {
1854	b43_phy_write(dev, reg: 0x047A, value: 0xC111);
1855	return;
1856	}
1857
1858	b43_phy_mask(dev, offset: 0x0036, mask: 0xFEFF);
1859	b43_phy_write(dev, reg: 0x002F, value: 0x0202);
1860	b43_phy_set(dev, offset: 0x047C, set: 0x0002);
1861	b43_phy_set(dev, offset: 0x047A, set: 0xF000);
1862	if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
1863	b43_phy_maskset(dev, offset: 0x047A, mask: 0xFF0F, set: 0x0010);
1864	b43_phy_set(dev, offset: 0x005D, set: 0x8000);
1865	b43_phy_maskset(dev, offset: 0x004E, mask: 0xFFC0, set: 0x0010);
1866	b43_phy_write(dev, reg: 0x002E, value: 0xC07F);
1867	b43_phy_set(dev, offset: 0x0036, set: 0x0400);
1868	} else {
1869	b43_phy_set(dev, offset: 0x0036, set: 0x0200);
1870	b43_phy_set(dev, offset: 0x0036, set: 0x0400);
1871	b43_phy_mask(dev, offset: 0x005D, mask: 0x7FFF);
1872	b43_phy_mask(dev, offset: 0x004F, mask: 0xFFFE);
1873	b43_phy_maskset(dev, offset: 0x004E, mask: 0xFFC0, set: 0x0010);
1874	b43_phy_write(dev, reg: 0x002E, value: 0xC07F);
1875	b43_phy_maskset(dev, offset: 0x047A, mask: 0xFF0F, set: 0x0010);
1876	}
1877	}
1878
1879	/* Hardware power control for G-PHY */
1880	static void b43_hardware_pctl_init_gphy(struct b43_wldev *dev)
1881	{
1882	struct b43_phy *phy = &dev->phy;
1883	struct b43_phy_g *gphy = phy->g;
1884
1885	if (!b43_has_hardware_pctl(dev)) {
1886	/* No hardware power control */
1887	b43_hf_write(dev, value: b43_hf_read(dev) & ~B43_HF_HWPCTL);
1888	return;
1889	}
1890
1891	b43_phy_maskset(dev, offset: 0x0036, mask: 0xFFC0, set: (gphy->tgt_idle_tssi - gphy->cur_idle_tssi));
1892	b43_phy_maskset(dev, offset: 0x0478, mask: 0xFF00, set: (gphy->tgt_idle_tssi - gphy->cur_idle_tssi));
1893	b43_gphy_tssi_power_lt_init(dev);
1894	b43_gphy_gain_lt_init(dev);
1895	b43_phy_mask(dev, offset: 0x0060, mask: 0xFFBF);
1896	b43_phy_write(dev, reg: 0x0014, value: 0x0000);
1897
1898	B43_WARN_ON(phy->rev < 6);
1899	b43_phy_set(dev, offset: 0x0478, set: 0x0800);
1900	b43_phy_mask(dev, offset: 0x0478, mask: 0xFEFF);
1901	b43_phy_mask(dev, offset: 0x0801, mask: 0xFFBF);
1902
1903	b43_gphy_dc_lt_init(dev, update_all: 1);
1904
1905	/* Enable hardware pctl in firmware. */
1906	b43_hf_write(dev, value: b43_hf_read(dev) | B43_HF_HWPCTL);
1907	}
1908
1909	/* Initialize B/G PHY power control */
1910	static void b43_phy_init_pctl(struct b43_wldev *dev)
1911	{
1912	struct b43_phy *phy = &dev->phy;
1913	struct b43_phy_g *gphy = phy->g;
1914	struct b43_rfatt old_rfatt;
1915	struct b43_bbatt old_bbatt;
1916	u8 old_tx_control = 0;
1917
1918	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
1919
1920	if ((dev->dev->board_vendor == SSB_BOARDVENDOR_BCM) &&
1921	(dev->dev->board_type == SSB_BOARD_BU4306))
1922	return;
1923
1924	b43_phy_write(dev, reg: 0x0028, value: 0x8018);
1925
1926	/* This does something with the Analog... */
1927	b43_write16(dev, B43_MMIO_PHY0, value: b43_read16(dev, B43_MMIO_PHY0)
1928	& 0xFFDF);
1929
1930	if (!phy->gmode)
1931	return;
1932	b43_hardware_pctl_early_init(dev);
1933	if (gphy->cur_idle_tssi == 0) {
1934	if (phy->radio_ver == 0x2050 && phy->analog == 0) {
1935	b43_radio_maskset(dev, offset: 0x0076, mask: 0x00F7, set: 0x0084);
1936	} else {
1937	struct b43_rfatt rfatt;
1938	struct b43_bbatt bbatt;
1939
1940	memcpy(&old_rfatt, &gphy->rfatt, sizeof(old_rfatt));
1941	memcpy(&old_bbatt, &gphy->bbatt, sizeof(old_bbatt));
1942	old_tx_control = gphy->tx_control;
1943
1944	bbatt.att = 11;
1945	if (phy->radio_rev == 8) {
1946	rfatt.att = 15;
1947	rfatt.with_padmix = true;
1948	} else {
1949	rfatt.att = 9;
1950	rfatt.with_padmix = false;
1951	}
1952	b43_set_txpower_g(dev, bbatt: &bbatt, rfatt: &rfatt, tx_control: 0);
1953	}
1954	b43_dummy_transmission(dev, ofdm: false, pa_on: true);
1955	gphy->cur_idle_tssi = b43_phy_read(dev, B43_PHY_ITSSI);
1956	if (B43_DEBUG) {
1957	/* Current-Idle-TSSI sanity check. */
1958	if (abs(gphy->cur_idle_tssi - gphy->tgt_idle_tssi) >= 20) {
1959	b43dbg(wl: dev->wl,
1960	fmt: "!WARNING! Idle-TSSI phy->cur_idle_tssi "
1961	"measuring failed. (cur=%d, tgt=%d). Disabling TX power "
1962	"adjustment.\n", gphy->cur_idle_tssi,
1963	gphy->tgt_idle_tssi);
1964	gphy->cur_idle_tssi = 0;
1965	}
1966	}
1967	if (phy->radio_ver == 0x2050 && phy->analog == 0) {
1968	b43_radio_mask(dev, offset: 0x0076, mask: 0xFF7B);
1969	} else {
1970	b43_set_txpower_g(dev, bbatt: &old_bbatt,
1971	rfatt: &old_rfatt, tx_control: old_tx_control);
1972	}
1973	}
1974	b43_hardware_pctl_init_gphy(dev);
1975	b43_shm_clear_tssi(dev);
1976	}
1977
1978	static void b43_phy_inita(struct b43_wldev *dev)
1979	{
1980	struct b43_phy *phy = &dev->phy;
1981
1982	might_sleep();
1983
1984	if (phy->rev >= 6) {
1985	if (b43_phy_read(dev, B43_PHY_ENCORE) & B43_PHY_ENCORE_EN)
1986	b43_phy_set(dev, B43_PHY_ENCORE, set: 0x0010);
1987	else
1988	b43_phy_mask(dev, B43_PHY_ENCORE, mask: ~0x1010);
1989	}
1990
1991	b43_wa_all(dev);
1992
1993	if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_PACTRL)
1994	b43_phy_maskset(dev, B43_PHY_OFDM(0x6E), mask: 0xE000, set: 0x3CF);
1995	}
1996
1997	static void b43_phy_initg(struct b43_wldev *dev)
1998	{
1999	struct b43_phy *phy = &dev->phy;
2000	struct b43_phy_g *gphy = phy->g;
2001	u16 tmp;
2002
2003	if (phy->rev == 1)
2004	b43_phy_initb5(dev);
2005	else
2006	b43_phy_initb6(dev);
2007
2008	if (phy->rev >= 2 || phy->gmode)
2009	b43_phy_inita(dev);
2010
2011	if (phy->rev >= 2) {
2012	b43_phy_write(dev, B43_PHY_ANALOGOVER, value: 0);
2013	b43_phy_write(dev, B43_PHY_ANALOGOVERVAL, value: 0);
2014	}
2015	if (phy->rev == 2) {
2016	b43_phy_write(dev, B43_PHY_RFOVER, value: 0);
2017	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xC0);
2018	}
2019	if (phy->rev > 5) {
2020	b43_phy_write(dev, B43_PHY_RFOVER, value: 0x400);
2021	b43_phy_write(dev, B43_PHY_PGACTL, value: 0xC0);
2022	}
2023	if (phy->gmode || phy->rev >= 2) {
2024	tmp = b43_phy_read(dev, B43_PHY_VERSION_OFDM);
2025	tmp &= B43_PHYVER_VERSION;
2026	if (tmp == 3 || tmp == 5) {
2027	b43_phy_write(dev, B43_PHY_OFDM(0xC2), value: 0x1816);
2028	b43_phy_write(dev, B43_PHY_OFDM(0xC3), value: 0x8006);
2029	}
2030	if (tmp == 5) {
2031	b43_phy_maskset(dev, B43_PHY_OFDM(0xCC), mask: 0x00FF, set: 0x1F00);
2032	}
2033	}
2034	if ((phy->rev <= 2 && phy->gmode) || phy->rev >= 2)
2035	b43_phy_write(dev, B43_PHY_OFDM(0x7E), value: 0x78);
2036	if (phy->radio_rev == 8) {
2037	b43_phy_set(dev, B43_PHY_EXTG(0x01), set: 0x80);
2038	b43_phy_set(dev, B43_PHY_OFDM(0x3E), set: 0x4);
2039	}
2040	if (has_loopback_gain(phy))
2041	b43_calc_loopback_gain(dev);
2042
2043	if (phy->radio_rev != 8) {
2044	if (gphy->initval == 0xFFFF)
2045	gphy->initval = b43_radio_init2050(dev);
2046	else
2047	b43_radio_write16(dev, reg: 0x0078, value: gphy->initval);
2048	}
2049	b43_lo_g_init(dev);
2050	if (has_tx_magnification(phy)) {
2051	b43_radio_write16(dev, reg: 0x52,
2052	value: (b43_radio_read16(dev, reg: 0x52) & 0xFF00)
2053	| gphy->lo_control->tx_bias | gphy->
2054	lo_control->tx_magn);
2055	} else {
2056	b43_radio_maskset(dev, offset: 0x52, mask: 0xFFF0, set: gphy->lo_control->tx_bias);
2057	}
2058	if (phy->rev >= 6) {
2059	b43_phy_maskset(dev, B43_PHY_CCK(0x36), mask: 0x0FFF, set: (gphy->lo_control->tx_bias << 12));
2060	}
2061	if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_PACTRL)
2062	b43_phy_write(dev, B43_PHY_CCK(0x2E), value: 0x8075);
2063	else
2064	b43_phy_write(dev, B43_PHY_CCK(0x2E), value: 0x807F);
2065	if (phy->rev < 2)
2066	b43_phy_write(dev, B43_PHY_CCK(0x2F), value: 0x101);
2067	else
2068	b43_phy_write(dev, B43_PHY_CCK(0x2F), value: 0x202);
2069	if (phy->gmode || phy->rev >= 2) {
2070	b43_lo_g_adjust(dev);
2071	b43_phy_write(dev, B43_PHY_LO_MASK, value: 0x8078);
2072	}
2073
2074	if (!(dev->dev->bus_sprom->boardflags_lo & B43_BFL_RSSI)) {
2075	/* The specs state to update the NRSSI LT with
2076	* the value 0x7FFFFFFF here. I think that is some weird
2077	* compiler optimization in the original driver.
2078	* Essentially, what we do here is resetting all NRSSI LT
2079	* entries to -32 (see the clamp_val() in nrssi_hw_update())
2080	*/
2081	b43_nrssi_hw_update(dev, val: 0xFFFF); //FIXME?
2082	b43_calc_nrssi_threshold(dev);
2083	} else if (phy->gmode || phy->rev >= 2) {
2084	if (gphy->nrssi[0] == -1000) {
2085	B43_WARN_ON(gphy->nrssi[1] != -1000);
2086	b43_calc_nrssi_slope(dev);
2087	} else
2088	b43_calc_nrssi_threshold(dev);
2089	}
2090	if (phy->radio_rev == 8)
2091	b43_phy_write(dev, B43_PHY_EXTG(0x05), value: 0x3230);
2092	b43_phy_init_pctl(dev);
2093	/* FIXME: The spec says in the following if, the 0 should be replaced
2094	'if OFDM may not be used in the current locale'
2095	but OFDM is legal everywhere */
2096	if ((dev->dev->chip_id == 0x4306
2097	&& dev->dev->chip_pkg == 2) || 0) {
2098	b43_phy_mask(dev, B43_PHY_CRS0, mask: 0xBFFF);
2099	b43_phy_mask(dev, B43_PHY_OFDM(0xC3), mask: 0x7FFF);
2100	}
2101	}
2102
2103	void b43_gphy_channel_switch(struct b43_wldev *dev,
2104	unsigned int channel,
2105	bool synthetic_pu_workaround)
2106	{
2107	if (synthetic_pu_workaround)
2108	b43_synth_pu_workaround(dev, channel);
2109
2110	b43_write16(dev, B43_MMIO_CHANNEL, value: channel2freq_bg(channel));
2111
2112	if (channel == 14) {
2113	if (dev->dev->bus_sprom->country_code ==
2114	SSB_SPROM1CCODE_JAPAN)
2115	b43_hf_write(dev,
2116	value: b43_hf_read(dev) & ~B43_HF_ACPR);
2117	else
2118	b43_hf_write(dev,
2119	value: b43_hf_read(dev) | B43_HF_ACPR);
2120	b43_write16(dev, B43_MMIO_CHANNEL_EXT,
2121	value: b43_read16(dev, B43_MMIO_CHANNEL_EXT)
2122	| (1 << 11));
2123	} else {
2124	b43_write16(dev, B43_MMIO_CHANNEL_EXT,
2125	value: b43_read16(dev, B43_MMIO_CHANNEL_EXT)
2126	& 0xF7BF);
2127	}
2128	}
2129
2130	static void default_baseband_attenuation(struct b43_wldev *dev,
2131	struct b43_bbatt *bb)
2132	{
2133	struct b43_phy *phy = &dev->phy;
2134
2135	if (phy->radio_ver == 0x2050 && phy->radio_rev < 6)
2136	bb->att = 0;
2137	else
2138	bb->att = 2;
2139	}
2140
2141	static void default_radio_attenuation(struct b43_wldev *dev,
2142	struct b43_rfatt *rf)
2143	{
2144	struct b43_bus_dev *bdev = dev->dev;
2145	struct b43_phy *phy = &dev->phy;
2146
2147	rf->with_padmix = false;
2148
2149	if (dev->dev->board_vendor == SSB_BOARDVENDOR_BCM &&
2150	dev->dev->board_type == SSB_BOARD_BCM4309G) {
2151	if (dev->dev->board_rev < 0x43) {
2152	rf->att = 2;
2153	return;
2154	} else if (dev->dev->board_rev < 0x51) {
2155	rf->att = 3;
2156	return;
2157	}
2158	}
2159
2160	switch (phy->radio_ver) {
2161	case 0x2053:
2162	switch (phy->radio_rev) {
2163	case 1:
2164	rf->att = 6;
2165	return;
2166	}
2167	break;
2168	case 0x2050:
2169	switch (phy->radio_rev) {
2170	case 0:
2171	rf->att = 5;
2172	return;
2173	case 1:
2174	if (phy->type == B43_PHYTYPE_G) {
2175	if (bdev->board_vendor == SSB_BOARDVENDOR_BCM
2176	&& bdev->board_type == SSB_BOARD_BCM4309G
2177	&& bdev->board_rev >= 30)
2178	rf->att = 3;
2179	else if (bdev->board_vendor ==
2180	SSB_BOARDVENDOR_BCM
2181	&& bdev->board_type ==
2182	SSB_BOARD_BU4306)
2183	rf->att = 3;
2184	else
2185	rf->att = 1;
2186	} else {
2187	if (bdev->board_vendor == SSB_BOARDVENDOR_BCM
2188	&& bdev->board_type == SSB_BOARD_BCM4309G
2189	&& bdev->board_rev >= 30)
2190	rf->att = 7;
2191	else
2192	rf->att = 6;
2193	}
2194	return;
2195	case 2:
2196	if (phy->type == B43_PHYTYPE_G) {
2197	if (bdev->board_vendor == SSB_BOARDVENDOR_BCM
2198	&& bdev->board_type == SSB_BOARD_BCM4309G
2199	&& bdev->board_rev >= 30)
2200	rf->att = 3;
2201	else if (bdev->board_vendor ==
2202	SSB_BOARDVENDOR_BCM
2203	&& bdev->board_type ==
2204	SSB_BOARD_BU4306)
2205	rf->att = 5;
2206	else if (bdev->chip_id == 0x4320)
2207	rf->att = 4;
2208	else
2209	rf->att = 3;
2210	} else
2211	rf->att = 6;
2212	return;
2213	case 3:
2214	rf->att = 5;
2215	return;
2216	case 4:
2217	case 5:
2218	rf->att = 1;
2219	return;
2220	case 6:
2221	case 7:
2222	rf->att = 5;
2223	return;
2224	case 8:
2225	rf->att = 0xA;
2226	rf->with_padmix = true;
2227	return;
2228	case 9:
2229	default:
2230	rf->att = 5;
2231	return;
2232	}
2233	}
2234	rf->att = 5;
2235	}
2236
2237	static u16 default_tx_control(struct b43_wldev *dev)
2238	{
2239	struct b43_phy *phy = &dev->phy;
2240
2241	if (phy->radio_ver != 0x2050)
2242	return 0;
2243	if (phy->radio_rev == 1)
2244	return B43_TXCTL_PA2DB | B43_TXCTL_TXMIX;
2245	if (phy->radio_rev < 6)
2246	return B43_TXCTL_PA2DB;
2247	if (phy->radio_rev == 8)
2248	return B43_TXCTL_TXMIX;
2249	return 0;
2250	}
2251
2252	static u8 b43_gphy_aci_detect(struct b43_wldev *dev, u8 channel)
2253	{
2254	struct b43_phy *phy = &dev->phy;
2255	struct b43_phy_g *gphy = phy->g;
2256	u8 ret = 0;
2257	u16 saved, rssi, temp;
2258	int i, j = 0;
2259
2260	saved = b43_phy_read(dev, reg: 0x0403);
2261	b43_switch_channel(dev, new_channel: channel);
2262	b43_phy_write(dev, reg: 0x0403, value: (saved & 0xFFF8) | 5);
2263	if (gphy->aci_hw_rssi)
2264	rssi = b43_phy_read(dev, reg: 0x048A) & 0x3F;
2265	else
2266	rssi = saved & 0x3F;
2267	/* clamp temp to signed 5bit */
2268	if (rssi > 32)
2269	rssi -= 64;
2270	for (i = 0; i < 100; i++) {
2271	temp = (b43_phy_read(dev, reg: 0x047F) >> 8) & 0x3F;
2272	if (temp > 32)
2273	temp -= 64;
2274	if (temp < rssi)
2275	j++;
2276	if (j >= 20)
2277	ret = 1;
2278	}
2279	b43_phy_write(dev, reg: 0x0403, value: saved);
2280
2281	return ret;
2282	}
2283
2284	static u8 b43_gphy_aci_scan(struct b43_wldev *dev)
2285	{
2286	struct b43_phy *phy = &dev->phy;
2287	u8 ret[13] = { 0 };
2288	unsigned int channel = phy->channel;
2289	unsigned int i, j, start, end;
2290
2291	if (!((phy->type == B43_PHYTYPE_G) && (phy->rev > 0)))
2292	return 0;
2293
2294	b43_phy_lock(dev);
2295	b43_radio_lock(dev);
2296	b43_phy_mask(dev, offset: 0x0802, mask: 0xFFFC);
2297	b43_phy_mask(dev, B43_PHY_G_CRS, mask: 0x7FFF);
2298	b43_set_all_gains(dev, first: 3, second: 8, third: 1);
2299
2300	start = (channel > 5) ? channel - 5 : 1;
2301	end = (channel + 5 < 14) ? channel + 5 : 13;
2302
2303	for (i = start; i <= end; i++) {
2304	if (abs(channel - i) > 2)
2305	ret[i - 1] = b43_gphy_aci_detect(dev, channel: i);
2306	}
2307	b43_switch_channel(dev, new_channel: channel);
2308	b43_phy_maskset(dev, offset: 0x0802, mask: 0xFFFC, set: 0x0003);
2309	b43_phy_mask(dev, offset: 0x0403, mask: 0xFFF8);
2310	b43_phy_set(dev, B43_PHY_G_CRS, set: 0x8000);
2311	b43_set_original_gains(dev);
2312	for (i = 0; i < 13; i++) {
2313	if (!ret[i])
2314	continue;
2315	end = (i + 5 < 13) ? i + 5 : 13;
2316	for (j = i; j < end; j++)
2317	ret[j] = 1;
2318	}
2319	b43_radio_unlock(dev);
2320	b43_phy_unlock(dev);
2321
2322	return ret[channel - 1];
2323	}
2324
2325	static s32 b43_tssi2dbm_ad(s32 num, s32 den)
2326	{
2327	if (num < 0)
2328	return num / den;
2329	else
2330	return (num + den / 2) / den;
2331	}
2332
2333	static s8 b43_tssi2dbm_entry(s8 entry[], u8 index,
2334	s16 pab0, s16 pab1, s16 pab2)
2335	{
2336	s32 m1, m2, f = 256, q, delta;
2337	s8 i = 0;
2338
2339	m1 = b43_tssi2dbm_ad(num: 16 * pab0 + index * pab1, den: 32);
2340	m2 = max(b43_tssi2dbm_ad(32768 + index * pab2, 256), 1);
2341	do {
2342	if (i > 15)
2343	return -EINVAL;
2344	q = b43_tssi2dbm_ad(num: f * 4096 -
2345	b43_tssi2dbm_ad(num: m2 * f, den: 16) * f, den: 2048);
2346	delta = abs(q - f);
2347	f = q;
2348	i++;
2349	} while (delta >= 2);
2350	entry[index] = clamp_val(b43_tssi2dbm_ad(m1 * f, 8192), -127, 128);
2351	return 0;
2352	}
2353
2354	u8 *b43_generate_dyn_tssi2dbm_tab(struct b43_wldev *dev,
2355	s16 pab0, s16 pab1, s16 pab2)
2356	{
2357	unsigned int i;
2358	u8 *tab;
2359	int err;
2360
2361	tab = kmalloc(size: 64, GFP_KERNEL);
2362	if (!tab) {
2363	b43err(wl: dev->wl, fmt: "Could not allocate memory "
2364	"for tssi2dbm table\n");
2365	return NULL;
2366	}
2367	for (i = 0; i < 64; i++) {
2368	err = b43_tssi2dbm_entry(entry: tab, index: i, pab0, pab1, pab2);
2369	if (err) {
2370	b43err(wl: dev->wl, fmt: "Could not generate "
2371	"tssi2dBm table\n");
2372	kfree(objp: tab);
2373	return NULL;
2374	}
2375	}
2376
2377	return tab;
2378	}
2379
2380	/* Initialise the TSSI->dBm lookup table */
2381	static int b43_gphy_init_tssi2dbm_table(struct b43_wldev *dev)
2382	{
2383	struct b43_phy *phy = &dev->phy;
2384	struct b43_phy_g *gphy = phy->g;
2385	s16 pab0, pab1, pab2;
2386
2387	pab0 = (s16) (dev->dev->bus_sprom->pa0b0);
2388	pab1 = (s16) (dev->dev->bus_sprom->pa0b1);
2389	pab2 = (s16) (dev->dev->bus_sprom->pa0b2);
2390
2391	B43_WARN_ON((dev->dev->chip_id == 0x4301) &&
2392	(phy->radio_ver != 0x2050)); /* Not supported anymore */
2393
2394	gphy->dyn_tssi_tbl = false;
2395
2396	if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
2397	pab0 != -1 && pab1 != -1 && pab2 != -1) {
2398	/* The pabX values are set in SPROM. Use them. */
2399	if ((s8) dev->dev->bus_sprom->itssi_bg != 0 &&
2400	(s8) dev->dev->bus_sprom->itssi_bg != -1) {
2401	gphy->tgt_idle_tssi =
2402	(s8) (dev->dev->bus_sprom->itssi_bg);
2403	} else
2404	gphy->tgt_idle_tssi = 62;
2405	gphy->tssi2dbm = b43_generate_dyn_tssi2dbm_tab(dev, pab0,
2406	pab1, pab2);
2407	if (!gphy->tssi2dbm)
2408	return -ENOMEM;
2409	gphy->dyn_tssi_tbl = true;
2410	} else {
2411	/* pabX values not set in SPROM. */
2412	gphy->tgt_idle_tssi = 52;
2413	gphy->tssi2dbm = b43_tssi2dbm_g_table;
2414	}
2415
2416	return 0;
2417	}
2418
2419	static int b43_gphy_op_allocate(struct b43_wldev *dev)
2420	{
2421	struct b43_phy_g *gphy;
2422	struct b43_txpower_lo_control *lo;
2423	int err;
2424
2425	gphy = kzalloc(size: sizeof(*gphy), GFP_KERNEL);
2426	if (!gphy) {
2427	err = -ENOMEM;
2428	goto error;
2429	}
2430	dev->phy.g = gphy;
2431
2432	lo = kzalloc(size: sizeof(*lo), GFP_KERNEL);
2433	if (!lo) {
2434	err = -ENOMEM;
2435	goto err_free_gphy;
2436	}
2437	gphy->lo_control = lo;
2438
2439	err = b43_gphy_init_tssi2dbm_table(dev);
2440	if (err)
2441	goto err_free_lo;
2442
2443	return 0;
2444
2445	err_free_lo:
2446	kfree(objp: lo);
2447	err_free_gphy:
2448	kfree(objp: gphy);
2449	error:
2450	return err;
2451	}
2452
2453	static void b43_gphy_op_prepare_structs(struct b43_wldev *dev)
2454	{
2455	struct b43_phy *phy = &dev->phy;
2456	struct b43_phy_g *gphy = phy->g;
2457	const void *tssi2dbm;
2458	int tgt_idle_tssi;
2459	struct b43_txpower_lo_control *lo;
2460	unsigned int i;
2461
2462	/* tssi2dbm table is constant, so it is initialized at alloc time.
2463	* Save a copy of the pointer. */
2464	tssi2dbm = gphy->tssi2dbm;
2465	tgt_idle_tssi = gphy->tgt_idle_tssi;
2466	/* Save the LO pointer. */
2467	lo = gphy->lo_control;
2468
2469	/* Zero out the whole PHY structure. */
2470	memset(gphy, 0, sizeof(*gphy));
2471
2472	/* Restore pointers. */
2473	gphy->tssi2dbm = tssi2dbm;
2474	gphy->tgt_idle_tssi = tgt_idle_tssi;
2475	gphy->lo_control = lo;
2476
2477	memset(gphy->minlowsig, 0xFF, sizeof(gphy->minlowsig));
2478
2479	/* NRSSI */
2480	for (i = 0; i < ARRAY_SIZE(gphy->nrssi); i++)
2481	gphy->nrssi[i] = -1000;
2482	for (i = 0; i < ARRAY_SIZE(gphy->nrssi_lt); i++)
2483	gphy->nrssi_lt[i] = i;
2484
2485	gphy->lofcal = 0xFFFF;
2486	gphy->initval = 0xFFFF;
2487
2488	gphy->interfmode = B43_INTERFMODE_NONE;
2489
2490	/* OFDM-table address caching. */
2491	gphy->ofdmtab_addr_direction = B43_OFDMTAB_DIRECTION_UNKNOWN;
2492
2493	gphy->average_tssi = 0xFF;
2494
2495	/* Local Osciallator structure */
2496	lo->tx_bias = 0xFF;
2497	INIT_LIST_HEAD(list: &lo->calib_list);
2498	}
2499
2500	static void b43_gphy_op_free(struct b43_wldev *dev)
2501	{
2502	struct b43_phy *phy = &dev->phy;
2503	struct b43_phy_g *gphy = phy->g;
2504
2505	kfree(objp: gphy->lo_control);
2506
2507	if (gphy->dyn_tssi_tbl)
2508	kfree(objp: gphy->tssi2dbm);
2509	gphy->dyn_tssi_tbl = false;
2510	gphy->tssi2dbm = NULL;
2511
2512	kfree(objp: gphy);
2513	dev->phy.g = NULL;
2514	}
2515
2516	static int b43_gphy_op_prepare_hardware(struct b43_wldev *dev)
2517	{
2518	struct b43_phy *phy = &dev->phy;
2519	struct b43_phy_g *gphy = phy->g;
2520	struct b43_txpower_lo_control *lo = gphy->lo_control;
2521
2522	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
2523
2524	default_baseband_attenuation(dev, bb: &gphy->bbatt);
2525	default_radio_attenuation(dev, rf: &gphy->rfatt);
2526	gphy->tx_control = (default_tx_control(dev) << 4);
2527	generate_rfatt_list(dev, list: &lo->rfatt_list);
2528	generate_bbatt_list(dev, list: &lo->bbatt_list);
2529
2530	/* Commit previous writes */
2531	b43_read32(dev, B43_MMIO_MACCTL);
2532
2533	if (phy->rev == 1) {
2534	/* Workaround: Temporarly disable gmode through the early init
2535	* phase, as the gmode stuff is not needed for phy rev 1 */
2536	phy->gmode = false;
2537	b43_wireless_core_reset(dev, gmode: 0);
2538	b43_phy_initg(dev);
2539	phy->gmode = true;
2540	b43_wireless_core_reset(dev, gmode: 1);
2541	}
2542
2543	return 0;
2544	}
2545
2546	static int b43_gphy_op_init(struct b43_wldev *dev)
2547	{
2548	b43_phy_initg(dev);
2549
2550	return 0;
2551	}
2552
2553	static void b43_gphy_op_exit(struct b43_wldev *dev)
2554	{
2555	b43_lo_g_cleanup(dev);
2556	}
2557
2558	static u16 b43_gphy_op_read(struct b43_wldev *dev, u16 reg)
2559	{
2560	b43_write16f(dev, B43_MMIO_PHY_CONTROL, value: reg);
2561	return b43_read16(dev, B43_MMIO_PHY_DATA);
2562	}
2563
2564	static void b43_gphy_op_write(struct b43_wldev *dev, u16 reg, u16 value)
2565	{
2566	b43_write16f(dev, B43_MMIO_PHY_CONTROL, value: reg);
2567	b43_write16(dev, B43_MMIO_PHY_DATA, value);
2568	}
2569
2570	static u16 b43_gphy_op_radio_read(struct b43_wldev *dev, u16 reg)
2571	{
2572	/* Register 1 is a 32-bit register. */
2573	B43_WARN_ON(reg == 1);
2574	/* G-PHY needs 0x80 for read access. */
2575	reg |= 0x80;
2576
2577	b43_write16f(dev, B43_MMIO_RADIO_CONTROL, value: reg);
2578	return b43_read16(dev, B43_MMIO_RADIO_DATA_LOW);
2579	}
2580
2581	static void b43_gphy_op_radio_write(struct b43_wldev *dev, u16 reg, u16 value)
2582	{
2583	/* Register 1 is a 32-bit register. */
2584	B43_WARN_ON(reg == 1);
2585
2586	b43_write16f(dev, B43_MMIO_RADIO_CONTROL, value: reg);
2587	b43_write16(dev, B43_MMIO_RADIO_DATA_LOW, value);
2588	}
2589
2590	static bool b43_gphy_op_supports_hwpctl(struct b43_wldev *dev)
2591	{
2592	return (dev->phy.rev >= 6);
2593	}
2594
2595	static void b43_gphy_op_software_rfkill(struct b43_wldev *dev,
2596	bool blocked)
2597	{
2598	struct b43_phy *phy = &dev->phy;
2599	struct b43_phy_g *gphy = phy->g;
2600	unsigned int channel;
2601
2602	might_sleep();
2603
2604	if (!blocked) {
2605	/* Turn radio ON */
2606	if (phy->radio_on)
2607	return;
2608
2609	b43_phy_write(dev, reg: 0x0015, value: 0x8000);
2610	b43_phy_write(dev, reg: 0x0015, value: 0xCC00);
2611	b43_phy_write(dev, reg: 0x0015, value: (phy->gmode ? 0x00C0 : 0x0000));
2612	if (gphy->radio_off_context.valid) {
2613	/* Restore the RFover values. */
2614	b43_phy_write(dev, B43_PHY_RFOVER,
2615	value: gphy->radio_off_context.rfover);
2616	b43_phy_write(dev, B43_PHY_RFOVERVAL,
2617	value: gphy->radio_off_context.rfoverval);
2618	gphy->radio_off_context.valid = false;
2619	}
2620	channel = phy->channel;
2621	b43_gphy_channel_switch(dev, channel: 6, synthetic_pu_workaround: 1);
2622	b43_gphy_channel_switch(dev, channel, synthetic_pu_workaround: 0);
2623	} else {
2624	/* Turn radio OFF */
2625	u16 rfover, rfoverval;
2626
2627	rfover = b43_phy_read(dev, B43_PHY_RFOVER);
2628	rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
2629	gphy->radio_off_context.rfover = rfover;
2630	gphy->radio_off_context.rfoverval = rfoverval;
2631	gphy->radio_off_context.valid = true;
2632	b43_phy_write(dev, B43_PHY_RFOVER, value: rfover | 0x008C);
2633	b43_phy_write(dev, B43_PHY_RFOVERVAL, value: rfoverval & 0xFF73);
2634	}
2635	}
2636
2637	static int b43_gphy_op_switch_channel(struct b43_wldev *dev,
2638	unsigned int new_channel)
2639	{
2640	if ((new_channel < 1) || (new_channel > 14))
2641	return -EINVAL;
2642	b43_gphy_channel_switch(dev, channel: new_channel, synthetic_pu_workaround: 0);
2643
2644	return 0;
2645	}
2646
2647	static unsigned int b43_gphy_op_get_default_chan(struct b43_wldev *dev)
2648	{
2649	return 1; /* Default to channel 1 */
2650	}
2651
2652	static void b43_gphy_op_set_rx_antenna(struct b43_wldev *dev, int antenna)
2653	{
2654	struct b43_phy *phy = &dev->phy;
2655	u16 tmp;
2656	int autodiv = 0;
2657
2658	if (antenna == B43_ANTENNA_AUTO0 || antenna == B43_ANTENNA_AUTO1)
2659	autodiv = 1;
2660
2661	b43_hf_write(dev, value: b43_hf_read(dev) & ~B43_HF_ANTDIVHELP);
2662
2663	b43_phy_maskset(dev, B43_PHY_BBANDCFG, mask: ~B43_PHY_BBANDCFG_RXANT,
2664	set: (autodiv ? B43_ANTENNA_AUTO1 : antenna) <<
2665	B43_PHY_BBANDCFG_RXANT_SHIFT);
2666
2667	if (autodiv) {
2668	tmp = b43_phy_read(dev, B43_PHY_ANTDWELL);
2669	if (antenna == B43_ANTENNA_AUTO1)
2670	tmp &= ~B43_PHY_ANTDWELL_AUTODIV1;
2671	else
2672	tmp |= B43_PHY_ANTDWELL_AUTODIV1;
2673	b43_phy_write(dev, B43_PHY_ANTDWELL, value: tmp);
2674	}
2675
2676	tmp = b43_phy_read(dev, B43_PHY_ANTWRSETT);
2677	if (autodiv)
2678	tmp |= B43_PHY_ANTWRSETT_ARXDIV;
2679	else
2680	tmp &= ~B43_PHY_ANTWRSETT_ARXDIV;
2681	b43_phy_write(dev, B43_PHY_ANTWRSETT, value: tmp);
2682
2683	if (autodiv)
2684	b43_phy_set(dev, B43_PHY_ANTWRSETT, B43_PHY_ANTWRSETT_ARXDIV);
2685	else {
2686	b43_phy_mask(dev, B43_PHY_ANTWRSETT,
2687	B43_PHY_ANTWRSETT_ARXDIV);
2688	}
2689
2690	if (phy->rev >= 2) {
2691	b43_phy_set(dev, B43_PHY_OFDM61, B43_PHY_OFDM61_10);
2692	b43_phy_maskset(dev, B43_PHY_DIVSRCHGAINBACK, mask: 0xFF00, set: 0x15);
2693
2694	if (phy->rev == 2)
2695	b43_phy_write(dev, B43_PHY_ADIVRELATED, value: 8);
2696	else
2697	b43_phy_maskset(dev, B43_PHY_ADIVRELATED, mask: 0xFF00, set: 8);
2698	}
2699	if (phy->rev >= 6)
2700	b43_phy_write(dev, B43_PHY_OFDM9B, value: 0xDC);
2701
2702	b43_hf_write(dev, value: b43_hf_read(dev) | B43_HF_ANTDIVHELP);
2703	}
2704
2705	static int b43_gphy_op_interf_mitigation(struct b43_wldev *dev,
2706	enum b43_interference_mitigation mode)
2707	{
2708	struct b43_phy *phy = &dev->phy;
2709	struct b43_phy_g *gphy = phy->g;
2710	int currentmode;
2711
2712	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
2713	if ((phy->rev == 0) || (!phy->gmode))
2714	return -ENODEV;
2715
2716	gphy->aci_wlan_automatic = false;
2717	switch (mode) {
2718	case B43_INTERFMODE_AUTOWLAN:
2719	gphy->aci_wlan_automatic = true;
2720	if (gphy->aci_enable)
2721	mode = B43_INTERFMODE_MANUALWLAN;
2722	else
2723	mode = B43_INTERFMODE_NONE;
2724	break;
2725	case B43_INTERFMODE_NONE:
2726	case B43_INTERFMODE_NONWLAN:
2727	case B43_INTERFMODE_MANUALWLAN:
2728	break;
2729	default:
2730	return -EINVAL;
2731	}
2732
2733	currentmode = gphy->interfmode;
2734	if (currentmode == mode)
2735	return 0;
2736	if (currentmode != B43_INTERFMODE_NONE)
2737	b43_radio_interference_mitigation_disable(dev, mode: currentmode);
2738
2739	if (mode == B43_INTERFMODE_NONE) {
2740	gphy->aci_enable = false;
2741	gphy->aci_hw_rssi = false;
2742	} else
2743	b43_radio_interference_mitigation_enable(dev, mode);
2744	gphy->interfmode = mode;
2745
2746	return 0;
2747	}
2748
2749	/* https://bcm-specs.sipsolutions.net/EstimatePowerOut
2750	* This function converts a TSSI value to dBm in Q5.2
2751	*/
2752	static s8 b43_gphy_estimate_power_out(struct b43_wldev *dev, s8 tssi)
2753	{
2754	struct b43_phy_g *gphy = dev->phy.g;
2755	s8 dbm;
2756	s32 tmp;
2757
2758	tmp = (gphy->tgt_idle_tssi - gphy->cur_idle_tssi + tssi);
2759	tmp = clamp_val(tmp, 0x00, 0x3F);
2760	dbm = gphy->tssi2dbm[tmp];
2761
2762	return dbm;
2763	}
2764
2765	static void b43_put_attenuation_into_ranges(struct b43_wldev *dev,
2766	int *_bbatt, int *_rfatt)
2767	{
2768	int rfatt = *_rfatt;
2769	int bbatt = *_bbatt;
2770	struct b43_txpower_lo_control *lo = dev->phy.g->lo_control;
2771
2772	/* Get baseband and radio attenuation values into their permitted ranges.
2773	* Radio attenuation affects power level 4 times as much as baseband. */
2774
2775	/* Range constants */
2776	const int rf_min = lo->rfatt_list.min_val;
2777	const int rf_max = lo->rfatt_list.max_val;
2778	const int bb_min = lo->bbatt_list.min_val;
2779	const int bb_max = lo->bbatt_list.max_val;
2780
2781	while (1) {
2782	if (rfatt > rf_max && bbatt > bb_max - 4)
2783	break; /* Can not get it into ranges */
2784	if (rfatt < rf_min && bbatt < bb_min + 4)
2785	break; /* Can not get it into ranges */
2786	if (bbatt > bb_max && rfatt > rf_max - 1)
2787	break; /* Can not get it into ranges */
2788	if (bbatt < bb_min && rfatt < rf_min + 1)
2789	break; /* Can not get it into ranges */
2790
2791	if (bbatt > bb_max) {
2792	bbatt -= 4;
2793	rfatt += 1;
2794	continue;
2795	}
2796	if (bbatt < bb_min) {
2797	bbatt += 4;
2798	rfatt -= 1;
2799	continue;
2800	}
2801	if (rfatt > rf_max) {
2802	rfatt -= 1;
2803	bbatt += 4;
2804	continue;
2805	}
2806	if (rfatt < rf_min) {
2807	rfatt += 1;
2808	bbatt -= 4;
2809	continue;
2810	}
2811	break;
2812	}
2813
2814	*_rfatt = clamp_val(rfatt, rf_min, rf_max);
2815	*_bbatt = clamp_val(bbatt, bb_min, bb_max);
2816	}
2817
2818	static void b43_gphy_op_adjust_txpower(struct b43_wldev *dev)
2819	{
2820	struct b43_phy *phy = &dev->phy;
2821	struct b43_phy_g *gphy = phy->g;
2822	int rfatt, bbatt;
2823	u8 tx_control;
2824
2825	b43_mac_suspend(dev);
2826
2827	/* Calculate the new attenuation values. */
2828	bbatt = gphy->bbatt.att;
2829	bbatt += gphy->bbatt_delta;
2830	rfatt = gphy->rfatt.att;
2831	rfatt += gphy->rfatt_delta;
2832
2833	b43_put_attenuation_into_ranges(dev, bbatt: &bbatt, rfatt: &rfatt);
2834	tx_control = gphy->tx_control;
2835	if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
2836	if (rfatt <= 1) {
2837	if (tx_control == 0) {
2838	tx_control =
2839	B43_TXCTL_PA2DB |
2840	B43_TXCTL_TXMIX;
2841	rfatt += 2;
2842	bbatt += 2;
2843	} else if (dev->dev->bus_sprom->
2844	boardflags_lo &
2845	B43_BFL_PACTRL) {
2846	bbatt += 4 * (rfatt - 2);
2847	rfatt = 2;
2848	}
2849	} else if (rfatt > 4 && tx_control) {
2850	tx_control = 0;
2851	if (bbatt < 3) {
2852	rfatt -= 3;
2853	bbatt += 2;
2854	} else {
2855	rfatt -= 2;
2856	bbatt -= 2;
2857	}
2858	}
2859	}
2860	/* Save the control values */
2861	gphy->tx_control = tx_control;
2862	b43_put_attenuation_into_ranges(dev, bbatt: &bbatt, rfatt: &rfatt);
2863	gphy->rfatt.att = rfatt;
2864	gphy->bbatt.att = bbatt;
2865
2866	if (b43_debug(dev, feature: B43_DBG_XMITPOWER))
2867	b43dbg(wl: dev->wl, fmt: "Adjusting TX power\n");
2868
2869	/* Adjust the hardware */
2870	b43_phy_lock(dev);
2871	b43_radio_lock(dev);
2872	b43_set_txpower_g(dev, bbatt: &gphy->bbatt, rfatt: &gphy->rfatt,
2873	tx_control: gphy->tx_control);
2874	b43_radio_unlock(dev);
2875	b43_phy_unlock(dev);
2876
2877	b43_mac_enable(dev);
2878	}
2879
2880	static enum b43_txpwr_result b43_gphy_op_recalc_txpower(struct b43_wldev *dev,
2881	bool ignore_tssi)
2882	{
2883	struct b43_phy *phy = &dev->phy;
2884	struct b43_phy_g *gphy = phy->g;
2885	unsigned int average_tssi;
2886	int cck_result, ofdm_result;
2887	int estimated_pwr, desired_pwr, pwr_adjust;
2888	int rfatt_delta, bbatt_delta;
2889	unsigned int max_pwr;
2890
2891	/* First get the average TSSI */
2892	cck_result = b43_phy_shm_tssi_read(dev, B43_SHM_SH_TSSI_CCK);
2893	ofdm_result = b43_phy_shm_tssi_read(dev, B43_SHM_SH_TSSI_OFDM_G);
2894	if ((cck_result < 0) && (ofdm_result < 0)) {
2895	/* No TSSI information available */
2896	if (!ignore_tssi)
2897	goto no_adjustment_needed;
2898	cck_result = 0;
2899	ofdm_result = 0;
2900	}
2901	if (cck_result < 0)
2902	average_tssi = ofdm_result;
2903	else if (ofdm_result < 0)
2904	average_tssi = cck_result;
2905	else
2906	average_tssi = (cck_result + ofdm_result) / 2;
2907	/* Merge the average with the stored value. */
2908	if (likely(gphy->average_tssi != 0xFF))
2909	average_tssi = (average_tssi + gphy->average_tssi) / 2;
2910	gphy->average_tssi = average_tssi;
2911	B43_WARN_ON(average_tssi >= B43_TSSI_MAX);
2912
2913	/* Estimate the TX power emission based on the TSSI */
2914	estimated_pwr = b43_gphy_estimate_power_out(dev, tssi: average_tssi);
2915
2916	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
2917	max_pwr = dev->dev->bus_sprom->maxpwr_bg;
2918	if (dev->dev->bus_sprom->boardflags_lo & B43_BFL_PACTRL)
2919	max_pwr -= 3; /* minus 0.75 */
2920	if (unlikely(max_pwr >= INT_TO_Q52(30/*dBm*/))) {
2921	b43warn(wl: dev->wl,
2922	fmt: "Invalid max-TX-power value in SPROM.\n");
2923	max_pwr = INT_TO_Q52(20); /* fake it */
2924	dev->dev->bus_sprom->maxpwr_bg = max_pwr;
2925	}
2926
2927	/* Get desired power (in Q5.2) */
2928	if (phy->desired_txpower < 0)
2929	desired_pwr = INT_TO_Q52(0);
2930	else
2931	desired_pwr = INT_TO_Q52(phy->desired_txpower);
2932	/* And limit it. max_pwr already is Q5.2 */
2933	desired_pwr = clamp_val(desired_pwr, 0, max_pwr);
2934	if (b43_debug(dev, feature: B43_DBG_XMITPOWER)) {
2935	b43dbg(wl: dev->wl,
2936	fmt: "[TX power] current = " Q52_FMT
2937	" dBm, desired = " Q52_FMT
2938	" dBm, max = " Q52_FMT "\n",
2939	Q52_ARG(estimated_pwr),
2940	Q52_ARG(desired_pwr),
2941	Q52_ARG(max_pwr));
2942	}
2943
2944	/* Calculate the adjustment delta. */
2945	pwr_adjust = desired_pwr - estimated_pwr;
2946	if (pwr_adjust == 0)
2947	goto no_adjustment_needed;
2948
2949	/* RF attenuation delta. */
2950	rfatt_delta = ((pwr_adjust + 7) / 8);
2951	/* Lower attenuation => Bigger power output. Negate it. */
2952	rfatt_delta = -rfatt_delta;
2953
2954	/* Baseband attenuation delta. */
2955	bbatt_delta = pwr_adjust / 2;
2956	/* Lower attenuation => Bigger power output. Negate it. */
2957	bbatt_delta = -bbatt_delta;
2958	/* RF att affects power level 4 times as much as
2959	* Baseband attennuation. Subtract it. */
2960	bbatt_delta -= 4 * rfatt_delta;
2961
2962	#if B43_DEBUG
2963	if (b43_debug(dev, feature: B43_DBG_XMITPOWER)) {
2964	int dbm = pwr_adjust < 0 ? -pwr_adjust : pwr_adjust;
2965	b43dbg(wl: dev->wl,
2966	fmt: "[TX power deltas] %s" Q52_FMT " dBm => "
2967	"bbatt-delta = %d, rfatt-delta = %d\n",
2968	(pwr_adjust < 0 ? "-" : ""), Q52_ARG(dbm),
2969	bbatt_delta, rfatt_delta);
2970	}
2971	#endif /* DEBUG */
2972
2973	/* So do we finally need to adjust something in hardware? */
2974	if ((rfatt_delta == 0) && (bbatt_delta == 0))
2975	goto no_adjustment_needed;
2976
2977	/* Save the deltas for later when we adjust the power. */
2978	gphy->bbatt_delta = bbatt_delta;
2979	gphy->rfatt_delta = rfatt_delta;
2980
2981	/* We need to adjust the TX power on the device. */
2982	return B43_TXPWR_RES_NEED_ADJUST;
2983
2984	no_adjustment_needed:
2985	return B43_TXPWR_RES_DONE;
2986	}
2987
2988	static void b43_gphy_op_pwork_15sec(struct b43_wldev *dev)
2989	{
2990	struct b43_phy *phy = &dev->phy;
2991	struct b43_phy_g *gphy = phy->g;
2992
2993	b43_mac_suspend(dev);
2994	//TODO: update_aci_moving_average
2995	if (gphy->aci_enable && gphy->aci_wlan_automatic) {
2996	if (!gphy->aci_enable && 1 /*TODO: not scanning? */ ) {
2997	if (0 /*TODO: bunch of conditions */ ) {
2998	phy->ops->interf_mitigation(dev,
2999	B43_INTERFMODE_MANUALWLAN);
3000	}
3001	} else if (0 /*TODO*/) {
3002	if (/*(aci_average > 1000) &&*/ !b43_gphy_aci_scan(dev))
3003	phy->ops->interf_mitigation(dev, B43_INTERFMODE_NONE);
3004	}
3005	} else if (gphy->interfmode == B43_INTERFMODE_NONWLAN &&
3006	phy->rev == 1) {
3007	//TODO: implement rev1 workaround
3008	}
3009	b43_lo_g_maintenance_work(dev);
3010	b43_mac_enable(dev);
3011	}
3012
3013	static void b43_gphy_op_pwork_60sec(struct b43_wldev *dev)
3014	{
3015	struct b43_phy *phy = &dev->phy;
3016
3017	if (!(dev->dev->bus_sprom->boardflags_lo & B43_BFL_RSSI))
3018	return;
3019
3020	b43_mac_suspend(dev);
3021	b43_calc_nrssi_slope(dev);
3022	if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 8)) {
3023	u8 old_chan = phy->channel;
3024
3025	/* VCO Calibration */
3026	if (old_chan >= 8)
3027	b43_switch_channel(dev, new_channel: 1);
3028	else
3029	b43_switch_channel(dev, new_channel: 13);
3030	b43_switch_channel(dev, new_channel: old_chan);
3031	}
3032	b43_mac_enable(dev);
3033	}
3034
3035	const struct b43_phy_operations b43_phyops_g = {
3036	.allocate = b43_gphy_op_allocate,
3037	.free = b43_gphy_op_free,
3038	.prepare_structs = b43_gphy_op_prepare_structs,
3039	.prepare_hardware = b43_gphy_op_prepare_hardware,
3040	.init = b43_gphy_op_init,
3041	.exit = b43_gphy_op_exit,
3042	.phy_read = b43_gphy_op_read,
3043	.phy_write = b43_gphy_op_write,
3044	.radio_read = b43_gphy_op_radio_read,
3045	.radio_write = b43_gphy_op_radio_write,
3046	.supports_hwpctl = b43_gphy_op_supports_hwpctl,
3047	.software_rfkill = b43_gphy_op_software_rfkill,
3048	.switch_analog = b43_phyop_switch_analog_generic,
3049	.switch_channel = b43_gphy_op_switch_channel,
3050	.get_default_chan = b43_gphy_op_get_default_chan,
3051	.set_rx_antenna = b43_gphy_op_set_rx_antenna,
3052	.interf_mitigation = b43_gphy_op_interf_mitigation,
3053	.recalc_txpower = b43_gphy_op_recalc_txpower,
3054	.adjust_txpower = b43_gphy_op_adjust_txpower,
3055	.pwork_15sec = b43_gphy_op_pwork_15sec,
3056	.pwork_60sec = b43_gphy_op_pwork_60sec,
3057	};
3058