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