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