/drivers/net/wireless/b43/phy_g.c

https://bitbucket.org/slukk/jb-tsm-kernel-4.2 · C · 3057 lines · 2614 code · 324 blank · 119 comment · 608 complexity · c3566ffd5deb78ee6c2f29f121c52936 MD5 · raw file

Large files are truncated click here to view the full file

    

  1. /*
    2. 

    3. 

  3.   Broadcom B43 wireless driver
    4. 

  4.   IEEE 802.11g PHY driver
    5. 

    6. 

  6.   Copyright (c) 2005 Martin Langer <martin-langer@gmx.de>,
    7. 

  7.   Copyright (c) 2005-2007 Stefano Brivio <stefano.brivio@polimi.it>
    8. 

  8.   Copyright (c) 2005-2008 Michael Buesch <mb@bu3sch.de>
    9. 

  9.   Copyright (c) 2005, 2006 Danny van Dyk <kugelfang@gentoo.org>
    10. 

  10.   Copyright (c) 2005, 2006 Andreas Jaggi <andreas.jaggi@waterwave.ch>
    11. 

    12. 

  12.   This program is free software; you can redistribute it and/or modify
    13. 

  13.   it under the terms of the GNU General Public License as published by
    14. 

  14.   the Free Software Foundation; either version 2 of the License, or
    15. 

  15.   (at your option) any later version.
    16. 

    17. 

  17.   This program is distributed in the hope that it will be useful,
    18. 

  18.   but WITHOUT ANY WARRANTY; without even the implied warranty of
    19. 

  19.   MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    20. 

  20.   GNU General Public License for more details.
    21. 

    22. 

  22.   You should have received a copy of the GNU General Public License
    23. 

  23.   along with this program; see the file COPYING.  If not, write to
    24. 

  24.   the Free Software Foundation, Inc., 51 Franklin Steet, Fifth Floor,
    25. 

  25.   Boston, MA 02110-1301, USA.
    26. 

    27. 

  27. */
    28. 

  28. 

  29. #include "b43.h"
    30. 

  30. #include "phy_g.h"
    31. 

  31. #include "phy_common.h"
    32. 

  32. #include "lo.h"
    33. 

  33. #include "main.h"
    34. 

  34. 

  35. #include <linux/bitrev.h>
    36. 

  36. #include <linux/slab.h>
    37. 

  37. 

  38. 

  39. static const s8 b43_tssi2dbm_g_table[] = {
    40. 

  40. 	77, 77, 77, 76,
    41. 

  41. 	76, 76, 75, 75,
    42. 

  42. 	74, 74, 73, 73,
    43. 

  43. 	73, 72, 72, 71,
    44. 

  44. 	71, 70, 70, 69,
    45. 

  45. 	68, 68, 67, 67,
    46. 

  46. 	66, 65, 65, 64,
    47. 

  47. 	63, 63, 62, 61,
    48. 

  48. 	60, 59, 58, 57,
    49. 

  49. 	56, 55, 54, 53,
    50. 

  50. 	52, 50, 49, 47,
    51. 

  51. 	45, 43, 40, 37,
    52. 

  52. 	33, 28, 22, 14,
    53. 

  53. 	5, -7, -20, -20,
    54. 

  54. 	-20, -20, -20, -20,
    55. 

  55. 	-20, -20, -20, -20,
    56. 

  56. };
    57. 

  57. 

  58. static const u8 b43_radio_channel_codes_bg[] = {
    59. 

  59. 	12, 17, 22, 27,
    60. 

  60. 	32, 37, 42, 47,
    61. 

  61. 	52, 57, 62, 67,
    62. 

  62. 	72, 84,
    63. 

  63. };
    64. 

  64. 

  65. 

  66. static void b43_calc_nrssi_threshold(struct b43_wldev *dev);
    67. 

  67. 

  68. 

  69. #define bitrev4(tmp) (bitrev8(tmp) >> 4)
    70. 

  70. 

  71. 

  72. /* Get the freq, as it has to be written to the device. */
    73. 

  73. static inline u16 channel2freq_bg(u8 channel)
    74. 

  74. {
    75. 

  75. 	B43_WARN_ON(!(channel >= 1 && channel <= 14));
    76. 

  76. 

  77. 	return b43_radio_channel_codes_bg[channel - 1];
    78. 

  78. }
    79. 

  79. 

  80. static void generate_rfatt_list(struct b43_wldev *dev,
    81. 

  81. 				struct b43_rfatt_list *list)
    82. 

  82. {
    83. 

  83. 	struct b43_phy *phy = &dev->phy;
    84. 

  84. 

  85. 	/* APHY.rev < 5 || GPHY.rev < 6 */
    86. 

  86. 	static const struct b43_rfatt rfatt_0[] = {
    87. 

  87. 		{.att = 3,.with_padmix = 0,},
    88. 

  88. 		{.att = 1,.with_padmix = 0,},
    89. 

  89. 		{.att = 5,.with_padmix = 0,},
    90. 

  90. 		{.att = 7,.with_padmix = 0,},
    91. 

  91. 		{.att = 9,.with_padmix = 0,},
    92. 

  92. 		{.att = 2,.with_padmix = 0,},
    93. 

  93. 		{.att = 0,.with_padmix = 0,},
    94. 

  94. 		{.att = 4,.with_padmix = 0,},
    95. 

  95. 		{.att = 6,.with_padmix = 0,},
    96. 

  96. 		{.att = 8,.with_padmix = 0,},
    97. 

  97. 		{.att = 1,.with_padmix = 1,},
    98. 

  98. 		{.att = 2,.with_padmix = 1,},
    99. 

  99. 		{.att = 3,.with_padmix = 1,},
    100. 

  100. 		{.att = 4,.with_padmix = 1,},
    101. 

  101. 	};
    102. 

  102. 	/* Radio.rev == 8 && Radio.version == 0x2050 */
    103. 

  103. 	static const struct b43_rfatt rfatt_1[] = {
    104. 

  104. 		{.att = 2,.with_padmix = 1,},
    105. 

  105. 		{.att = 4,.with_padmix = 1,},
    106. 

  106. 		{.att = 6,.with_padmix = 1,},
    107. 

  107. 		{.att = 8,.with_padmix = 1,},
    108. 

  108. 		{.att = 10,.with_padmix = 1,},
    109. 

  109. 		{.att = 12,.with_padmix = 1,},
    110. 

  110. 		{.att = 14,.with_padmix = 1,},
    111. 

  111. 	};
    112. 

  112. 	/* Otherwise */
    113. 

  113. 	static const struct b43_rfatt rfatt_2[] = {
    114. 

  114. 		{.att = 0,.with_padmix = 1,},
    115. 

  115. 		{.att = 2,.with_padmix = 1,},
    116. 

  116. 		{.att = 4,.with_padmix = 1,},
    117. 

  117. 		{.att = 6,.with_padmix = 1,},
    118. 

  118. 		{.att = 8,.with_padmix = 1,},
    119. 

  119. 		{.att = 9,.with_padmix = 1,},
    120. 

  120. 		{.att = 9,.with_padmix = 1,},
    121. 

  121. 	};
    122. 

  122. 

  123. 	if (!b43_has_hardware_pctl(dev)) {
    124. 

  124. 		/* Software pctl */
    125. 

  125. 		list->list = rfatt_0;
    126. 

  126. 		list->len = ARRAY_SIZE(rfatt_0);
    127. 

  127. 		list->min_val = 0;
    128. 

  128. 		list->max_val = 9;
    129. 

  129. 		return;
    130. 

  130. 	}
    131. 

  131. 	if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
    132. 

  132. 		/* Hardware pctl */
    133. 

  133. 		list->list = rfatt_1;
    134. 

  134. 		list->len = ARRAY_SIZE(rfatt_1);
    135. 

  135. 		list->min_val = 0;
    136. 

  136. 		list->max_val = 14;
    137. 

  137. 		return;
    138. 

  138. 	}
    139. 

  139. 	/* Hardware pctl */
    140. 

  140. 	list->list = rfatt_2;
    141. 

  141. 	list->len = ARRAY_SIZE(rfatt_2);
    142. 

  142. 	list->min_val = 0;
    143. 

  143. 	list->max_val = 9;
    144. 

  144. }
    145. 

  145. 

  146. static void generate_bbatt_list(struct b43_wldev *dev,
    147. 

  147. 				struct b43_bbatt_list *list)
    148. 

  148. {
    149. 

  149. 	static const struct b43_bbatt bbatt_0[] = {
    150. 

  150. 		{.att = 0,},
    151. 

  151. 		{.att = 1,},
    152. 

  152. 		{.att = 2,},
    153. 

  153. 		{.att = 3,},
    154. 

  154. 		{.att = 4,},
    155. 

  155. 		{.att = 5,},
    156. 

  156. 		{.att = 6,},
    157. 

  157. 		{.att = 7,},
    158. 

  158. 		{.att = 8,},
    159. 

  159. 	};
    160. 

  160. 

  161. 	list->list = bbatt_0;
    162. 

  162. 	list->len = ARRAY_SIZE(bbatt_0);
    163. 

  163. 	list->min_val = 0;
    164. 

  164. 	list->max_val = 8;
    165. 

  165. }
    166. 

  166. 

  167. static void b43_shm_clear_tssi(struct b43_wldev *dev)
    168. 

  168. {
    169. 

  169. 	b43_shm_write16(dev, B43_SHM_SHARED, 0x0058, 0x7F7F);
    170. 

  170. 	b43_shm_write16(dev, B43_SHM_SHARED, 0x005a, 0x7F7F);
    171. 

  171. 	b43_shm_write16(dev, B43_SHM_SHARED, 0x0070, 0x7F7F);
    172. 

  172. 	b43_shm_write16(dev, B43_SHM_SHARED, 0x0072, 0x7F7F);
    173. 

  173. }
    174. 

  174. 

  175. /* Synthetic PU workaround */
    176. 

  176. static void b43_synth_pu_workaround(struct b43_wldev *dev, u8 channel)
    177. 

  177. {
    178. 

  178. 	struct b43_phy *phy = &dev->phy;
    179. 

  179. 

  180. 	might_sleep();
    181. 

  181. 

  182. 	if (phy->radio_ver != 0x2050 || phy->radio_rev >= 6) {
    183. 

  183. 		/* We do not need the workaround. */
    184. 

  184. 		return;
    185. 

  185. 	}
    186. 

  186. 

  187. 	if (channel <= 10) {
    188. 

  188. 		b43_write16(dev, B43_MMIO_CHANNEL,
    189. 

  189. 			    channel2freq_bg(channel + 4));
    190. 

  190. 	} else {
    191. 

  191. 		b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(1));
    192. 

  192. 	}
    193. 

  193. 	msleep(1);
    194. 

  194. 	b43_write16(dev, B43_MMIO_CHANNEL, channel2freq_bg(channel));
    195. 

  195. }
    196. 

  196. 

  197. /* Set the baseband attenuation value on chip. */
    198. 

  198. void b43_gphy_set_baseband_attenuation(struct b43_wldev *dev,
    199. 

  199. 				       u16 baseband_attenuation)
    200. 

  200. {
    201. 

  201. 	struct b43_phy *phy = &dev->phy;
    202. 

  202. 

  203. 	if (phy->analog == 0) {
    204. 

  204. 		b43_write16(dev, B43_MMIO_PHY0, (b43_read16(dev, B43_MMIO_PHY0)
    205. 

  205. 						 & 0xFFF0) |
    206. 

  206. 			    baseband_attenuation);
    207. 

  207. 	} else if (phy->analog > 1) {
    208. 

  208. 		b43_phy_maskset(dev, B43_PHY_DACCTL, 0xFFC3, (baseband_attenuation << 2));
    209. 

  209. 	} else {
    210. 

  210. 		b43_phy_maskset(dev, B43_PHY_DACCTL, 0xFF87, (baseband_attenuation << 3));
    211. 

  211. 	}
    212. 

  212. }
    213. 

  213. 

  214. /* Adjust the transmission power output (G-PHY) */
    215. 

  215. static void b43_set_txpower_g(struct b43_wldev *dev,
    216. 

  216. 			      const struct b43_bbatt *bbatt,
    217. 

  217. 			      const struct b43_rfatt *rfatt, u8 tx_control)
    218. 

  218. {
    219. 

  219. 	struct b43_phy *phy = &dev->phy;
    220. 

  220. 	struct b43_phy_g *gphy = phy->g;
    221. 

  221. 	struct b43_txpower_lo_control *lo = gphy->lo_control;
    222. 

  222. 	u16 bb, rf;
    223. 

  223. 	u16 tx_bias, tx_magn;
    224. 

  224. 

  225. 	bb = bbatt->att;
    226. 

  226. 	rf = rfatt->att;
    227. 

  227. 	tx_bias = lo->tx_bias;
    228. 

  228. 	tx_magn = lo->tx_magn;
    229. 

  229. 	if (unlikely(tx_bias == 0xFF))
    230. 

  230. 		tx_bias = 0;
    231. 

  231. 

  232. 	/* Save the values for later. Use memmove, because it's valid
    233. 

  233. 	 * to pass &gphy->rfatt as rfatt pointer argument. Same for bbatt. */
    234. 

  234. 	gphy->tx_control = tx_control;
    235. 

  235. 	memmove(&gphy->rfatt, rfatt, sizeof(*rfatt));
    236. 

  236. 	gphy->rfatt.with_padmix = !!(tx_control & B43_TXCTL_TXMIX);
    237. 

  237. 	memmove(&gphy->bbatt, bbatt, sizeof(*bbatt));
    238. 

  238. 

  239. 	if (b43_debug(dev, B43_DBG_XMITPOWER)) {
    240. 

  240. 		b43dbg(dev->wl, "Tuning TX-power to bbatt(%u), "
    241. 

  241. 		       "rfatt(%u), tx_control(0x%02X), "
    242. 

  242. 		       "tx_bias(0x%02X), tx_magn(0x%02X)\n",
    243. 

  243. 		       bb, rf, tx_control, tx_bias, tx_magn);
    244. 

  244. 	}
    245. 

  245. 

  246. 	b43_gphy_set_baseband_attenuation(dev, bb);
    247. 

  247. 	b43_shm_write16(dev, B43_SHM_SHARED, B43_SHM_SH_RFATT, rf);
    248. 

  248. 	if (phy->radio_ver == 0x2050 && phy->radio_rev == 8) {
    249. 

  249. 		b43_radio_write16(dev, 0x43,
    250. 

  250. 				  (rf & 0x000F) | (tx_control & 0x0070));
    251. 

  251. 	} else {
    252. 

  252. 		b43_radio_maskset(dev, 0x43, 0xFFF0, (rf & 0x000F));
    253. 

  253. 		b43_radio_maskset(dev, 0x52, ~0x0070, (tx_control & 0x0070));
    254. 

  254. 	}
    255. 

  255. 	if (has_tx_magnification(phy)) {
    256. 

  256. 		b43_radio_write16(dev, 0x52, tx_magn | tx_bias);
    257. 

  257. 	} else {
    258. 

  258. 		b43_radio_maskset(dev, 0x52, 0xFFF0, (tx_bias & 0x000F));
    259. 

  259. 	}
    260. 

  260. 	b43_lo_g_adjust(dev);
    261. 

  261. }
    262. 

  262. 

  263. /* GPHY_TSSI_Power_Lookup_Table_Init */
    264. 

  264. static void b43_gphy_tssi_power_lt_init(struct b43_wldev *dev)
    265. 

  265. {
    266. 

  266. 	struct b43_phy_g *gphy = dev->phy.g;
    267. 

  267. 	int i;
    268. 

  268. 	u16 value;
    269. 

  269. 

  270. 	for (i = 0; i < 32; i++)
    271. 

  271. 		b43_ofdmtab_write16(dev, 0x3C20, i, gphy->tssi2dbm[i]);
    272. 

  272. 	for (i = 32; i < 64; i++)
    273. 

  273. 		b43_ofdmtab_write16(dev, 0x3C00, i - 32, gphy->tssi2dbm[i]);
    274. 

  274. 	for (i = 0; i < 64; i += 2) {
    275. 

  275. 		value = (u16) gphy->tssi2dbm[i];
    276. 

  276. 		value |= ((u16) gphy->tssi2dbm[i + 1]) << 8;
    277. 

  277. 		b43_phy_write(dev, 0x380 + (i / 2), value);
    278. 

  278. 	}
    279. 

  279. }
    280. 

  280. 

  281. /* GPHY_Gain_Lookup_Table_Init */
    282. 

  282. static void b43_gphy_gain_lt_init(struct b43_wldev *dev)
    283. 

  283. {
    284. 

  284. 	struct b43_phy *phy = &dev->phy;
    285. 

  285. 	struct b43_phy_g *gphy = phy->g;
    286. 

  286. 	struct b43_txpower_lo_control *lo = gphy->lo_control;
    287. 

  287. 	u16 nr_written = 0;
    288. 

  288. 	u16 tmp;
    289. 

  289. 	u8 rf, bb;
    290. 

  290. 

  291. 	for (rf = 0; rf < lo->rfatt_list.len; rf++) {
    292. 

  292. 		for (bb = 0; bb < lo->bbatt_list.len; bb++) {
    293. 

  293. 			if (nr_written >= 0x40)
    294. 

  294. 				return;
    295. 

  295. 			tmp = lo->bbatt_list.list[bb].att;
    296. 

  296. 			tmp <<= 8;
    297. 

  297. 			if (phy->radio_rev == 8)
    298. 

  298. 				tmp |= 0x50;
    299. 

  299. 			else
    300. 

  300. 				tmp |= 0x40;
    301. 

  301. 			tmp |= lo->rfatt_list.list[rf].att;
    302. 

  302. 			b43_phy_write(dev, 0x3C0 + nr_written, tmp);
    303. 

  303. 			nr_written++;
    304. 

  304. 		}
    305. 

  305. 	}
    306. 

  306. }
    307. 

  307. 

  308. static void b43_set_all_gains(struct b43_wldev *dev,
    309. 

  309. 			      s16 first, s16 second, s16 third)
    310. 

  310. {
    311. 

  311. 	struct b43_phy *phy = &dev->phy;
    312. 

  312. 	u16 i;
    313. 

  313. 	u16 start = 0x08, end = 0x18;
    314. 

  314. 	u16 tmp;
    315. 

  315. 	u16 table;
    316. 

  316. 

  317. 	if (phy->rev <= 1) {
    318. 

  318. 		start = 0x10;
    319. 

  319. 		end = 0x20;
    320. 

  320. 	}
    321. 

  321. 

  322. 	table = B43_OFDMTAB_GAINX;
    323. 

  323. 	if (phy->rev <= 1)
    324. 

  324. 		table = B43_OFDMTAB_GAINX_R1;
    325. 

  325. 	for (i = 0; i < 4; i++)
    326. 

  326. 		b43_ofdmtab_write16(dev, table, i, first);
    327. 

  327. 

  328. 	for (i = start; i < end; i++)
    329. 

  329. 		b43_ofdmtab_write16(dev, table, i, second);
    330. 

  330. 

  331. 	if (third != -1) {
    332. 

  332. 		tmp = ((u16) third << 14) | ((u16) third << 6);
    333. 

  333. 		b43_phy_maskset(dev, 0x04A0, 0xBFBF, tmp);
    334. 

  334. 		b43_phy_maskset(dev, 0x04A1, 0xBFBF, tmp);
    335. 

  335. 		b43_phy_maskset(dev, 0x04A2, 0xBFBF, tmp);
    336. 

  336. 	}
    337. 

  337. 	b43_dummy_transmission(dev, false, true);
    338. 

  338. }
    339. 

  339. 

  340. static void b43_set_original_gains(struct b43_wldev *dev)
    341. 

  341. {
    342. 

  342. 	struct b43_phy *phy = &dev->phy;
    343. 

  343. 	u16 i, tmp;
    344. 

  344. 	u16 table;
    345. 

  345. 	u16 start = 0x0008, end = 0x0018;
    346. 

  346. 

  347. 	if (phy->rev <= 1) {
    348. 

  348. 		start = 0x0010;
    349. 

  349. 		end = 0x0020;
    350. 

  350. 	}
    351. 

  351. 

  352. 	table = B43_OFDMTAB_GAINX;
    353. 

  353. 	if (phy->rev <= 1)
    354. 

  354. 		table = B43_OFDMTAB_GAINX_R1;
    355. 

  355. 	for (i = 0; i < 4; i++) {
    356. 

  356. 		tmp = (i & 0xFFFC);
    357. 

  357. 		tmp |= (i & 0x0001) << 1;
    358. 

  358. 		tmp |= (i & 0x0002) >> 1;
    359. 

  359. 

  360. 		b43_ofdmtab_write16(dev, table, i, tmp);
    361. 

  361. 	}
    362. 

  362. 

  363. 	for (i = start; i < end; i++)
    364. 

  364. 		b43_ofdmtab_write16(dev, table, i, i - start);
    365. 

  365. 

  366. 	b43_phy_maskset(dev, 0x04A0, 0xBFBF, 0x4040);
    367. 

  367. 	b43_phy_maskset(dev, 0x04A1, 0xBFBF, 0x4040);
    368. 

  368. 	b43_phy_maskset(dev, 0x04A2, 0xBFBF, 0x4000);
    369. 

  369. 	b43_dummy_transmission(dev, false, true);
    370. 

  370. }
    371. 

  371. 

  372. /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
    373. 

  373. static void b43_nrssi_hw_write(struct b43_wldev *dev, u16 offset, s16 val)
    374. 

  374. {
    375. 

  375. 	b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
    376. 

  376. 	b43_phy_write(dev, B43_PHY_NRSSILT_DATA, (u16) val);
    377. 

  377. }
    378. 

  378. 

  379. /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
    380. 

  380. static s16 b43_nrssi_hw_read(struct b43_wldev *dev, u16 offset)
    381. 

  381. {
    382. 

  382. 	u16 val;
    383. 

  383. 

  384. 	b43_phy_write(dev, B43_PHY_NRSSILT_CTRL, offset);
    385. 

  385. 	val = b43_phy_read(dev, B43_PHY_NRSSILT_DATA);
    386. 

  386. 

  387. 	return (s16) val;
    388. 

  388. }
    389. 

  389. 

  390. /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
    391. 

  391. static void b43_nrssi_hw_update(struct b43_wldev *dev, u16 val)
    392. 

  392. {
    393. 

  393. 	u16 i;
    394. 

  394. 	s16 tmp;
    395. 

  395. 

  396. 	for (i = 0; i < 64; i++) {
    397. 

  397. 		tmp = b43_nrssi_hw_read(dev, i);
    398. 

  398. 		tmp -= val;
    399. 

  399. 		tmp = clamp_val(tmp, -32, 31);
    400. 

  400. 		b43_nrssi_hw_write(dev, i, tmp);
    401. 

  401. 	}
    402. 

  402. }
    403. 

  403. 

  404. /* http://bcm-specs.sipsolutions.net/NRSSILookupTable */
    405. 

  405. static void b43_nrssi_mem_update(struct b43_wldev *dev)
    406. 

  406. {
    407. 

  407. 	struct b43_phy_g *gphy = dev->phy.g;
    408. 

  408. 	s16 i, delta;
    409. 

  409. 	s32 tmp;
    410. 

  410. 

  411. 	delta = 0x1F - gphy->nrssi[0];
    412. 

  412. 	for (i = 0; i < 64; i++) {
    413. 

  413. 		tmp = (i - delta) * gphy->nrssislope;
    414. 

  414. 		tmp /= 0x10000;
    415. 

  415. 		tmp += 0x3A;
    416. 

  416. 		tmp = clamp_val(tmp, 0, 0x3F);
    417. 

  417. 		gphy->nrssi_lt[i] = tmp;
    418. 

  418. 	}
    419. 

  419. }
    420. 

  420. 

  421. static void b43_calc_nrssi_offset(struct b43_wldev *dev)
    422. 

  422. {
    423. 

  423. 	struct b43_phy *phy = &dev->phy;
    424. 

  424. 	u16 backup[20] = { 0 };
    425. 

  425. 	s16 v47F;
    426. 

  426. 	u16 i;
    427. 

  427. 	u16 saved = 0xFFFF;
    428. 

  428. 

  429. 	backup[0] = b43_phy_read(dev, 0x0001);
    430. 

  430. 	backup[1] = b43_phy_read(dev, 0x0811);
    431. 

  431. 	backup[2] = b43_phy_read(dev, 0x0812);
    432. 

  432. 	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
    433. 

  433. 		backup[3] = b43_phy_read(dev, 0x0814);
    434. 

  434. 		backup[4] = b43_phy_read(dev, 0x0815);
    435. 

  435. 	}
    436. 

  436. 	backup[5] = b43_phy_read(dev, 0x005A);
    437. 

  437. 	backup[6] = b43_phy_read(dev, 0x0059);
    438. 

  438. 	backup[7] = b43_phy_read(dev, 0x0058);
    439. 

  439. 	backup[8] = b43_phy_read(dev, 0x000A);
    440. 

  440. 	backup[9] = b43_phy_read(dev, 0x0003);
    441. 

  441. 	backup[10] = b43_radio_read16(dev, 0x007A);
    442. 

  442. 	backup[11] = b43_radio_read16(dev, 0x0043);
    443. 

  443. 

  444. 	b43_phy_mask(dev, 0x0429, 0x7FFF);
    445. 

  445. 	b43_phy_maskset(dev, 0x0001, 0x3FFF, 0x4000);
    446. 

  446. 	b43_phy_set(dev, 0x0811, 0x000C);
    447. 

  447. 	b43_phy_maskset(dev, 0x0812, 0xFFF3, 0x0004);
    448. 

  448. 	b43_phy_mask(dev, 0x0802, ~(0x1 | 0x2));
    449. 

  449. 	if (phy->rev >= 6) {
    450. 

  450. 		backup[12] = b43_phy_read(dev, 0x002E);
    451. 

  451. 		backup[13] = b43_phy_read(dev, 0x002F);
    452. 

  452. 		backup[14] = b43_phy_read(dev, 0x080F);
    453. 

  453. 		backup[15] = b43_phy_read(dev, 0x0810);
    454. 

  454. 		backup[16] = b43_phy_read(dev, 0x0801);
    455. 

  455. 		backup[17] = b43_phy_read(dev, 0x0060);
    456. 

  456. 		backup[18] = b43_phy_read(dev, 0x0014);
    457. 

  457. 		backup[19] = b43_phy_read(dev, 0x0478);
    458. 

  458. 

  459. 		b43_phy_write(dev, 0x002E, 0);
    460. 

  460. 		b43_phy_write(dev, 0x002F, 0);
    461. 

  461. 		b43_phy_write(dev, 0x080F, 0);
    462. 

  462. 		b43_phy_write(dev, 0x0810, 0);
    463. 

  463. 		b43_phy_set(dev, 0x0478, 0x0100);
    464. 

  464. 		b43_phy_set(dev, 0x0801, 0x0040);
    465. 

  465. 		b43_phy_set(dev, 0x0060, 0x0040);
    466. 

  466. 		b43_phy_set(dev, 0x0014, 0x0200);
    467. 

  467. 	}
    468. 

  468. 	b43_radio_set(dev, 0x007A, 0x0070);
    469. 

  469. 	b43_radio_set(dev, 0x007A, 0x0080);
    470. 

  470. 	udelay(30);
    471. 

  471. 

  472. 	v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
    473. 

  473. 	if (v47F >= 0x20)
    474. 

  474. 		v47F -= 0x40;
    475. 

  475. 	if (v47F == 31) {
    476. 

  476. 		for (i = 7; i >= 4; i--) {
    477. 

  477. 			b43_radio_write16(dev, 0x007B, i);
    478. 

  478. 			udelay(20);
    479. 

  479. 			v47F =
    480. 

  480. 			    (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
    481. 

  481. 			if (v47F >= 0x20)
    482. 

  482. 				v47F -= 0x40;
    483. 

  483. 			if (v47F < 31 && saved == 0xFFFF)
    484. 

  484. 				saved = i;
    485. 

  485. 		}
    486. 

  486. 		if (saved == 0xFFFF)
    487. 

  487. 			saved = 4;
    488. 

  488. 	} else {
    489. 

  489. 		b43_radio_mask(dev, 0x007A, 0x007F);
    490. 

  490. 		if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
    491. 

  491. 			b43_phy_set(dev, 0x0814, 0x0001);
    492. 

  492. 			b43_phy_mask(dev, 0x0815, 0xFFFE);
    493. 

  493. 		}
    494. 

  494. 		b43_phy_set(dev, 0x0811, 0x000C);
    495. 

  495. 		b43_phy_set(dev, 0x0812, 0x000C);
    496. 

  496. 		b43_phy_set(dev, 0x0811, 0x0030);
    497. 

  497. 		b43_phy_set(dev, 0x0812, 0x0030);
    498. 

  498. 		b43_phy_write(dev, 0x005A, 0x0480);
    499. 

  499. 		b43_phy_write(dev, 0x0059, 0x0810);
    500. 

  500. 		b43_phy_write(dev, 0x0058, 0x000D);
    501. 

  501. 		if (phy->rev == 0) {
    502. 

  502. 			b43_phy_write(dev, 0x0003, 0x0122);
    503. 

  503. 		} else {
    504. 

  504. 			b43_phy_set(dev, 0x000A, 0x2000);
    505. 

  505. 		}
    506. 

  506. 		if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
    507. 

  507. 			b43_phy_set(dev, 0x0814, 0x0004);
    508. 

  508. 			b43_phy_mask(dev, 0x0815, 0xFFFB);
    509. 

  509. 		}
    510. 

  510. 		b43_phy_maskset(dev, 0x0003, 0xFF9F, 0x0040);
    511. 

  511. 		b43_radio_set(dev, 0x007A, 0x000F);
    512. 

  512. 		b43_set_all_gains(dev, 3, 0, 1);
    513. 

  513. 		b43_radio_maskset(dev, 0x0043, 0x00F0, 0x000F);
    514. 

  514. 		udelay(30);
    515. 

  515. 		v47F = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
    516. 

  516. 		if (v47F >= 0x20)
    517. 

  517. 			v47F -= 0x40;
    518. 

  518. 		if (v47F == -32) {
    519. 

  519. 			for (i = 0; i < 4; i++) {
    520. 

  520. 				b43_radio_write16(dev, 0x007B, i);
    521. 

  521. 				udelay(20);
    522. 

  522. 				v47F =
    523. 

  523. 				    (s16) ((b43_phy_read(dev, 0x047F) >> 8) &
    524. 

  524. 					   0x003F);
    525. 

  525. 				if (v47F >= 0x20)
    526. 

  526. 					v47F -= 0x40;
    527. 

  527. 				if (v47F > -31 && saved == 0xFFFF)
    528. 

  528. 					saved = i;
    529. 

  529. 			}
    530. 

  530. 			if (saved == 0xFFFF)
    531. 

  531. 				saved = 3;
    532. 

  532. 		} else
    533. 

  533. 			saved = 0;
    534. 

  534. 	}
    535. 

  535. 	b43_radio_write16(dev, 0x007B, saved);
    536. 

  536. 

  537. 	if (phy->rev >= 6) {
    538. 

  538. 		b43_phy_write(dev, 0x002E, backup[12]);
    539. 

  539. 		b43_phy_write(dev, 0x002F, backup[13]);
    540. 

  540. 		b43_phy_write(dev, 0x080F, backup[14]);
    541. 

  541. 		b43_phy_write(dev, 0x0810, backup[15]);
    542. 

  542. 	}
    543. 

  543. 	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
    544. 

  544. 		b43_phy_write(dev, 0x0814, backup[3]);
    545. 

  545. 		b43_phy_write(dev, 0x0815, backup[4]);
    546. 

  546. 	}
    547. 

  547. 	b43_phy_write(dev, 0x005A, backup[5]);
    548. 

  548. 	b43_phy_write(dev, 0x0059, backup[6]);
    549. 

  549. 	b43_phy_write(dev, 0x0058, backup[7]);
    550. 

  550. 	b43_phy_write(dev, 0x000A, backup[8]);
    551. 

  551. 	b43_phy_write(dev, 0x0003, backup[9]);
    552. 

  552. 	b43_radio_write16(dev, 0x0043, backup[11]);
    553. 

  553. 	b43_radio_write16(dev, 0x007A, backup[10]);
    554. 

  554. 	b43_phy_write(dev, 0x0802, b43_phy_read(dev, 0x0802) | 0x1 | 0x2);
    555. 

  555. 	b43_phy_set(dev, 0x0429, 0x8000);
    556. 

  556. 	b43_set_original_gains(dev);
    557. 

  557. 	if (phy->rev >= 6) {
    558. 

  558. 		b43_phy_write(dev, 0x0801, backup[16]);
    559. 

  559. 		b43_phy_write(dev, 0x0060, backup[17]);
    560. 

  560. 		b43_phy_write(dev, 0x0014, backup[18]);
    561. 

  561. 		b43_phy_write(dev, 0x0478, backup[19]);
    562. 

  562. 	}
    563. 

  563. 	b43_phy_write(dev, 0x0001, backup[0]);
    564. 

  564. 	b43_phy_write(dev, 0x0812, backup[2]);
    565. 

  565. 	b43_phy_write(dev, 0x0811, backup[1]);
    566. 

  566. }
    567. 

  567. 

  568. static void b43_calc_nrssi_slope(struct b43_wldev *dev)
    569. 

  569. {
    570. 

  570. 	struct b43_phy *phy = &dev->phy;
    571. 

  571. 	struct b43_phy_g *gphy = phy->g;
    572. 

  572. 	u16 backup[18] = { 0 };
    573. 

  573. 	u16 tmp;
    574. 

  574. 	s16 nrssi0, nrssi1;
    575. 

  575. 

  576. 	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
    577. 

  577. 

  578. 	if (phy->radio_rev >= 9)
    579. 

  579. 		return;
    580. 

  580. 	if (phy->radio_rev == 8)
    581. 

  581. 		b43_calc_nrssi_offset(dev);
    582. 

  582. 

  583. 	b43_phy_mask(dev, B43_PHY_G_CRS, 0x7FFF);
    584. 

  584. 	b43_phy_mask(dev, 0x0802, 0xFFFC);
    585. 

  585. 	backup[7] = b43_read16(dev, 0x03E2);
    586. 

  586. 	b43_write16(dev, 0x03E2, b43_read16(dev, 0x03E2) | 0x8000);
    587. 

  587. 	backup[0] = b43_radio_read16(dev, 0x007A);
    588. 

  588. 	backup[1] = b43_radio_read16(dev, 0x0052);
    589. 

  589. 	backup[2] = b43_radio_read16(dev, 0x0043);
    590. 

  590. 	backup[3] = b43_phy_read(dev, 0x0015);
    591. 

  591. 	backup[4] = b43_phy_read(dev, 0x005A);
    592. 

  592. 	backup[5] = b43_phy_read(dev, 0x0059);
    593. 

  593. 	backup[6] = b43_phy_read(dev, 0x0058);
    594. 

  594. 	backup[8] = b43_read16(dev, 0x03E6);
    595. 

  595. 	backup[9] = b43_read16(dev, B43_MMIO_CHANNEL_EXT);
    596. 

  596. 	if (phy->rev >= 3) {
    597. 

  597. 		backup[10] = b43_phy_read(dev, 0x002E);
    598. 

  598. 		backup[11] = b43_phy_read(dev, 0x002F);
    599. 

  599. 		backup[12] = b43_phy_read(dev, 0x080F);
    600. 

  600. 		backup[13] = b43_phy_read(dev, B43_PHY_G_LO_CONTROL);
    601. 

  601. 		backup[14] = b43_phy_read(dev, 0x0801);
    602. 

  602. 		backup[15] = b43_phy_read(dev, 0x0060);
    603. 

  603. 		backup[16] = b43_phy_read(dev, 0x0014);
    604. 

  604. 		backup[17] = b43_phy_read(dev, 0x0478);
    605. 

  605. 		b43_phy_write(dev, 0x002E, 0);
    606. 

  606. 		b43_phy_write(dev, B43_PHY_G_LO_CONTROL, 0);
    607. 

  607. 		switch (phy->rev) {
    608. 

  608. 		case 4:
    609. 

  609. 		case 6:
    610. 

  610. 		case 7:
    611. 

  611. 			b43_phy_set(dev, 0x0478, 0x0100);
    612. 

  612. 			b43_phy_set(dev, 0x0801, 0x0040);
    613. 

  613. 			break;
    614. 

  614. 		case 3:
    615. 

  615. 		case 5:
    616. 

  616. 			b43_phy_mask(dev, 0x0801, 0xFFBF);
    617. 

  617. 			break;
    618. 

  618. 		}
    619. 

  619. 		b43_phy_set(dev, 0x0060, 0x0040);
    620. 

  620. 		b43_phy_set(dev, 0x0014, 0x0200);
    621. 

  621. 	}
    622. 

  622. 	b43_radio_set(dev, 0x007A, 0x0070);
    623. 

  623. 	b43_set_all_gains(dev, 0, 8, 0);
    624. 

  624. 	b43_radio_mask(dev, 0x007A, 0x00F7);
    625. 

  625. 	if (phy->rev >= 2) {
    626. 

  626. 		b43_phy_maskset(dev, 0x0811, 0xFFCF, 0x0030);
    627. 

  627. 		b43_phy_maskset(dev, 0x0812, 0xFFCF, 0x0010);
    628. 

  628. 	}
    629. 

  629. 	b43_radio_set(dev, 0x007A, 0x0080);
    630. 

  630. 	udelay(20);
    631. 

  631. 

  632. 	nrssi0 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
    633. 

  633. 	if (nrssi0 >= 0x0020)
    634. 

  634. 		nrssi0 -= 0x0040;
    635. 

  635. 

  636. 	b43_radio_mask(dev, 0x007A, 0x007F);
    637. 

  637. 	if (phy->rev >= 2) {
    638. 

  638. 		b43_phy_maskset(dev, 0x0003, 0xFF9F, 0x0040);
    639. 

  639. 	}
    640. 

  640. 

  641. 	b43_write16(dev, B43_MMIO_CHANNEL_EXT,
    642. 

  642. 		    b43_read16(dev, B43_MMIO_CHANNEL_EXT)
    643. 

  643. 		    | 0x2000);
    644. 

  644. 	b43_radio_set(dev, 0x007A, 0x000F);
    645. 

  645. 	b43_phy_write(dev, 0x0015, 0xF330);
    646. 

  646. 	if (phy->rev >= 2) {
    647. 

  647. 		b43_phy_maskset(dev, 0x0812, 0xFFCF, 0x0020);
    648. 

  648. 		b43_phy_maskset(dev, 0x0811, 0xFFCF, 0x0020);
    649. 

  649. 	}
    650. 

  650. 

  651. 	b43_set_all_gains(dev, 3, 0, 1);
    652. 

  652. 	if (phy->radio_rev == 8) {
    653. 

  653. 		b43_radio_write16(dev, 0x0043, 0x001F);
    654. 

  654. 	} else {
    655. 

  655. 		tmp = b43_radio_read16(dev, 0x0052) & 0xFF0F;
    656. 

  656. 		b43_radio_write16(dev, 0x0052, tmp | 0x0060);
    657. 

  657. 		tmp = b43_radio_read16(dev, 0x0043) & 0xFFF0;
    658. 

  658. 		b43_radio_write16(dev, 0x0043, tmp | 0x0009);
    659. 

  659. 	}
    660. 

  660. 	b43_phy_write(dev, 0x005A, 0x0480);
    661. 

  661. 	b43_phy_write(dev, 0x0059, 0x0810);
    662. 

  662. 	b43_phy_write(dev, 0x0058, 0x000D);
    663. 

  663. 	udelay(20);
    664. 

  664. 	nrssi1 = (s16) ((b43_phy_read(dev, 0x047F) >> 8) & 0x003F);
    665. 

  665. 	if (nrssi1 >= 0x0020)
    666. 

  666. 		nrssi1 -= 0x0040;
    667. 

  667. 	if (nrssi0 == nrssi1)
    668. 

  668. 		gphy->nrssislope = 0x00010000;
    669. 

  669. 	else
    670. 

  670. 		gphy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
    671. 

  671. 	if (nrssi0 >= -4) {
    672. 

  672. 		gphy->nrssi[0] = nrssi1;
    673. 

  673. 		gphy->nrssi[1] = nrssi0;
    674. 

  674. 	}
    675. 

  675. 	if (phy->rev >= 3) {
    676. 

  676. 		b43_phy_write(dev, 0x002E, backup[10]);
    677. 

  677. 		b43_phy_write(dev, 0x002F, backup[11]);
    678. 

  678. 		b43_phy_write(dev, 0x080F, backup[12]);
    679. 

  679. 		b43_phy_write(dev, B43_PHY_G_LO_CONTROL, backup[13]);
    680. 

  680. 	}
    681. 

  681. 	if (phy->rev >= 2) {
    682. 

  682. 		b43_phy_mask(dev, 0x0812, 0xFFCF);
    683. 

  683. 		b43_phy_mask(dev, 0x0811, 0xFFCF);
    684. 

  684. 	}
    685. 

  685. 

  686. 	b43_radio_write16(dev, 0x007A, backup[0]);
    687. 

  687. 	b43_radio_write16(dev, 0x0052, backup[1]);
    688. 

  688. 	b43_radio_write16(dev, 0x0043, backup[2]);
    689. 

  689. 	b43_write16(dev, 0x03E2, backup[7]);
    690. 

  690. 	b43_write16(dev, 0x03E6, backup[8]);
    691. 

  691. 	b43_write16(dev, B43_MMIO_CHANNEL_EXT, backup[9]);
    692. 

  692. 	b43_phy_write(dev, 0x0015, backup[3]);
    693. 

  693. 	b43_phy_write(dev, 0x005A, backup[4]);
    694. 

  694. 	b43_phy_write(dev, 0x0059, backup[5]);
    695. 

  695. 	b43_phy_write(dev, 0x0058, backup[6]);
    696. 

  696. 	b43_synth_pu_workaround(dev, phy->channel);
    697. 

  697. 	b43_phy_set(dev, 0x0802, (0x0001 | 0x0002));
    698. 

  698. 	b43_set_original_gains(dev);
    699. 

  699. 	b43_phy_set(dev, B43_PHY_G_CRS, 0x8000);
    700. 

  700. 	if (phy->rev >= 3) {
    701. 

  701. 		b43_phy_write(dev, 0x0801, backup[14]);
    702. 

  702. 		b43_phy_write(dev, 0x0060, backup[15]);
    703. 

  703. 		b43_phy_write(dev, 0x0014, backup[16]);
    704. 

  704. 		b43_phy_write(dev, 0x0478, backup[17]);
    705. 

  705. 	}
    706. 

  706. 	b43_nrssi_mem_update(dev);
    707. 

  707. 	b43_calc_nrssi_threshold(dev);
    708. 

  708. }
    709. 

  709. 

  710. static void b43_calc_nrssi_threshold(struct b43_wldev *dev)
    711. 

  711. {
    712. 

  712. 	struct b43_phy *phy = &dev->phy;
    713. 

  713. 	struct b43_phy_g *gphy = phy->g;
    714. 

  714. 	s32 a, b;
    715. 

  715. 	s16 tmp16;
    716. 

  716. 	u16 tmp_u16;
    717. 

  717. 

  718. 	B43_WARN_ON(phy->type != B43_PHYTYPE_G);
    719. 

  719. 

  720. 	if (!phy->gmode ||
    721. 

  721. 	    !(dev->sdev->bus->sprom.boardflags_lo & B43_BFL_RSSI)) {
    722. 

  722. 		tmp16 = b43_nrssi_hw_read(dev, 0x20);
    723. 

  723. 		if (tmp16 >= 0x20)
    724. 

  724. 			tmp16 -= 0x40;
    725. 

  725. 		if (tmp16 < 3) {
    726. 

  726. 			b43_phy_maskset(dev, 0x048A, 0xF000, 0x09EB);
    727. 

  727. 		} else {
    728. 

  728. 			b43_phy_maskset(dev, 0x048A, 0xF000, 0x0AED);
    729. 

  729. 		}
    730. 

  730. 	} else {
    731. 

  731. 		if (gphy->interfmode == B43_INTERFMODE_NONWLAN) {
    732. 

  732. 			a = 0xE;
    733. 

  733. 			b = 0xA;
    734. 

  734. 		} else if (!gphy->aci_wlan_automatic && gphy->aci_enable) {
    735. 

  735. 			a = 0x13;
    736. 

  736. 			b = 0x12;
    737. 

  737. 		} else {
    738. 

  738. 			a = 0xE;
    739. 

  739. 			b = 0x11;
    740. 

  740. 		}
    741. 

  741. 

  742. 		a = a * (gphy->nrssi[1] - gphy->nrssi[0]);
    743. 

  743. 		a += (gphy->nrssi[0] << 6);
    744. 

  744. 		if (a < 32)
    745. 

  745. 			a += 31;
    746. 

  746. 		else
    747. 

  747. 			a += 32;
    748. 

  748. 		a = a >> 6;
    749. 

  749. 		a = clamp_val(a, -31, 31);
    750. 

  750. 

  751. 		b = b * (gphy->nrssi[1] - gphy->nrssi[0]);
    752. 

  752. 		b += (gphy->nrssi[0] << 6);
    753. 

  753. 		if (b < 32)
    754. 

  754. 			b += 31;
    755. 

  755. 		else
    756. 

  756. 			b += 32;
    757. 

  757. 		b = b >> 6;
    758. 

  758. 		b = clamp_val(b, -31, 31);
    759. 

  759. 

  760. 		tmp_u16 = b43_phy_read(dev, 0x048A) & 0xF000;
    761. 

  761. 		tmp_u16 |= ((u32) b & 0x0000003F);
    762. 

  762. 		tmp_u16 |= (((u32) a & 0x0000003F) << 6);
    763. 

  763. 		b43_phy_write(dev, 0x048A, tmp_u16);
    764. 

  764. 	}
    765. 

  765. }
    766. 

  766. 

  767. /* Stack implementation to save/restore values from the
    768. 

  768.  * interference mitigation code.
    769. 

  769.  * It is save to restore values in random order.
    770. 

  770.  */
    771. 

  771. static void _stack_save(u32 *_stackptr, size_t *stackidx,
    772. 

  772. 			u8 id, u16 offset, u16 value)
    773. 

  773. {
    774. 

  774. 	u32 *stackptr = &(_stackptr[*stackidx]);
    775. 

  775. 

  776. 	B43_WARN_ON(offset & 0xF000);
    777. 

  777. 	B43_WARN_ON(id & 0xF0);
    778. 

  778. 	*stackptr = offset;
    779. 

  779. 	*stackptr |= ((u32) id) << 12;
    780. 

  780. 	*stackptr |= ((u32) value) << 16;
    781. 

  781. 	(*stackidx)++;
    782. 

  782. 	B43_WARN_ON(*stackidx >= B43_INTERFSTACK_SIZE);
    783. 

  783. }
    784. 

  784. 

  785. static u16 _stack_restore(u32 *stackptr, u8 id, u16 offset)
    786. 

  786. {
    787. 

  787. 	size_t i;
    788. 

  788. 

  789. 	B43_WARN_ON(offset & 0xF000);
    790. 

  790. 	B43_WARN_ON(id & 0xF0);
    791. 

  791. 	for (i = 0; i < B43_INTERFSTACK_SIZE; i++, stackptr++) {
    792. 

  792. 		if ((*stackptr & 0x00000FFF) != offset)
    793. 

  793. 			continue;
    794. 

  794. 		if (((*stackptr & 0x0000F000) >> 12) != id)
    795. 

  795. 			continue;
    796. 

  796. 		return ((*stackptr & 0xFFFF0000) >> 16);
    797. 

  797. 	}
    798. 

  798. 	B43_WARN_ON(1);
    799. 

  799. 

  800. 	return 0;
    801. 

  801. }
    802. 

  802. 

  803. #define phy_stacksave(offset)					\
    804. 

  804. 	do {							\
    805. 

  805. 		_stack_save(stack, &stackidx, 0x1, (offset),	\
    806. 

  806. 			    b43_phy_read(dev, (offset)));	\
    807. 

  807. 	} while (0)
    808. 

  808. #define phy_stackrestore(offset)				\
    809. 

  809. 	do {							\
    810. 

  810. 		b43_phy_write(dev, (offset),		\
    811. 

  811. 				  _stack_restore(stack, 0x1,	\
    812. 

  812. 						 (offset)));	\
    813. 

  813. 	} while (0)
    814. 

  814. #define radio_stacksave(offset)						\
    815. 

  815. 	do {								\
    816. 

  816. 		_stack_save(stack, &stackidx, 0x2, (offset),		\
    817. 

  817. 			    b43_radio_read16(dev, (offset)));	\
    818. 

  818. 	} while (0)
    819. 

  819. #define radio_stackrestore(offset)					\
    820. 

  820. 	do {								\
    821. 

  821. 		b43_radio_write16(dev, (offset),			\
    822. 

  822. 				      _stack_restore(stack, 0x2,	\
    823. 

  823. 						     (offset)));	\
    824. 

  824. 	} while (0)
    825. 

  825. #define ofdmtab_stacksave(table, offset)			\
    826. 

  826. 	do {							\
    827. 

  827. 		_stack_save(stack, &stackidx, 0x3, (offset)|(table),	\
    828. 

  828. 			    b43_ofdmtab_read16(dev, (table), (offset)));	\
    829. 

  829. 	} while (0)
    830. 

  830. #define ofdmtab_stackrestore(table, offset)			\
    831. 

  831. 	do {							\
    832. 

  832. 		b43_ofdmtab_write16(dev, (table),	(offset),	\
    833. 

  833. 				  _stack_restore(stack, 0x3,	\
    834. 

  834. 						 (offset)|(table)));	\
    835. 

  835. 	} while (0)
    836. 

  836. 

  837. static void
    838. 

  838. b43_radio_interference_mitigation_enable(struct b43_wldev *dev, int mode)
    839. 

  839. {
    840. 

  840. 	struct b43_phy *phy = &dev->phy;
    841. 

  841. 	struct b43_phy_g *gphy = phy->g;
    842. 

  842. 	u16 tmp, flipped;
    843. 

  843. 	size_t stackidx = 0;
    844. 

  844. 	u32 *stack = gphy->interfstack;
    845. 

  845. 

  846. 	switch (mode) {
    847. 

  847. 	case B43_INTERFMODE_NONWLAN:
    848. 

  848. 		if (phy->rev != 1) {
    849. 

  849. 			b43_phy_set(dev, 0x042B, 0x0800);
    850. 

  850. 			b43_phy_mask(dev, B43_PHY_G_CRS, ~0x4000);
    851. 

  851. 			break;
    852. 

  852. 		}
    853. 

  853. 		radio_stacksave(0x0078);
    854. 

  854. 		tmp = (b43_radio_read16(dev, 0x0078) & 0x001E);
    855. 

  855. 		B43_WARN_ON(tmp > 15);
    856. 

  856. 		flipped = bitrev4(tmp);
    857. 

  857. 		if (flipped < 10 && flipped >= 8)
    858. 

  858. 			flipped = 7;
    859. 

  859. 		else if (flipped >= 10)
    860. 

  860. 			flipped -= 3;
    861. 

  861. 		flipped = (bitrev4(flipped) << 1) | 0x0020;
    862. 

  862. 		b43_radio_write16(dev, 0x0078, flipped);
    863. 

  863. 

  864. 		b43_calc_nrssi_threshold(dev);
    865. 

  865. 

  866. 		phy_stacksave(0x0406);
    867. 

  867. 		b43_phy_write(dev, 0x0406, 0x7E28);
    868. 

  868. 

  869. 		b43_phy_set(dev, 0x042B, 0x0800);
    870. 

  870. 		b43_phy_set(dev, B43_PHY_RADIO_BITFIELD, 0x1000);
    871. 

  871. 

  872. 		phy_stacksave(0x04A0);
    873. 

  873. 		b43_phy_maskset(dev, 0x04A0, 0xC0C0, 0x0008);
    874. 

  874. 		phy_stacksave(0x04A1);
    875. 

  875. 		b43_phy_maskset(dev, 0x04A1, 0xC0C0, 0x0605);
    876. 

  876. 		phy_stacksave(0x04A2);
    877. 

  877. 		b43_phy_maskset(dev, 0x04A2, 0xC0C0, 0x0204);
    878. 

  878. 		phy_stacksave(0x04A8);
    879. 

  879. 		b43_phy_maskset(dev, 0x04A8, 0xC0C0, 0x0803);
    880. 

  880. 		phy_stacksave(0x04AB);
    881. 

  881. 		b43_phy_maskset(dev, 0x04AB, 0xC0C0, 0x0605);
    882. 

  882. 

  883. 		phy_stacksave(0x04A7);
    884. 

  884. 		b43_phy_write(dev, 0x04A7, 0x0002);
    885. 

  885. 		phy_stacksave(0x04A3);
    886. 

  886. 		b43_phy_write(dev, 0x04A3, 0x287A);
    887. 

  887. 		phy_stacksave(0x04A9);
    888. 

  888. 		b43_phy_write(dev, 0x04A9, 0x2027);
    889. 

  889. 		phy_stacksave(0x0493);
    890. 

  890. 		b43_phy_write(dev, 0x0493, 0x32F5);
    891. 

  891. 		phy_stacksave(0x04AA);
    892. 

  892. 		b43_phy_write(dev, 0x04AA, 0x2027);
    893. 

  893. 		phy_stacksave(0x04AC);
    894. 

  894. 		b43_phy_write(dev, 0x04AC, 0x32F5);
    895. 

  895. 		break;
    896. 

  896. 	case B43_INTERFMODE_MANUALWLAN:
    897. 

  897. 		if (b43_phy_read(dev, 0x0033) & 0x0800)
    898. 

  898. 			break;
    899. 

  899. 

  900. 		gphy->aci_enable = 1;
    901. 

  901. 

  902. 		phy_stacksave(B43_PHY_RADIO_BITFIELD);
    903. 

  903. 		phy_stacksave(B43_PHY_G_CRS);
    904. 

  904. 		if (phy->rev < 2) {
    905. 

  905. 			phy_stacksave(0x0406);
    906. 

  906. 		} else {
    907. 

  907. 			phy_stacksave(0x04C0);
    908. 

  908. 			phy_stacksave(0x04C1);
    909. 

  909. 		}
    910. 

  910. 		phy_stacksave(0x0033);
    911. 

  911. 		phy_stacksave(0x04A7);
    912. 

  912. 		phy_stacksave(0x04A3);
    913. 

  913. 		phy_stacksave(0x04A9);
    914. 

  914. 		phy_stacksave(0x04AA);
    915. 

  915. 		phy_stacksave(0x04AC);
    916. 

  916. 		phy_stacksave(0x0493);
    917. 

  917. 		phy_stacksave(0x04A1);
    918. 

  918. 		phy_stacksave(0x04A0);
    919. 

  919. 		phy_stacksave(0x04A2);
    920. 

  920. 		phy_stacksave(0x048A);
    921. 

  921. 		phy_stacksave(0x04A8);
    922. 

  922. 		phy_stacksave(0x04AB);
    923. 

  923. 		if (phy->rev == 2) {
    924. 

  924. 			phy_stacksave(0x04AD);
    925. 

  925. 			phy_stacksave(0x04AE);
    926. 

  926. 		} else if (phy->rev >= 3) {
    927. 

  927. 			phy_stacksave(0x04AD);
    928. 

  928. 			phy_stacksave(0x0415);
    929. 

  929. 			phy_stacksave(0x0416);
    930. 

  930. 			phy_stacksave(0x0417);
    931. 

  931. 			ofdmtab_stacksave(0x1A00, 0x2);
    932. 

  932. 			ofdmtab_stacksave(0x1A00, 0x3);
    933. 

  933. 		}
    934. 

  934. 		phy_stacksave(0x042B);
    935. 

  935. 		phy_stacksave(0x048C);
    936. 

  936. 

  937. 		b43_phy_mask(dev, B43_PHY_RADIO_BITFIELD, ~0x1000);
    938. 

  938. 		b43_phy_maskset(dev, B43_PHY_G_CRS, 0xFFFC, 0x0002);
    939. 

  939. 

  940. 		b43_phy_write(dev, 0x0033, 0x0800);
    941. 

  941. 		b43_phy_write(dev, 0x04A3, 0x2027);
    942. 

  942. 		b43_phy_write(dev, 0x04A9, 0x1CA8);
    943. 

  943. 		b43_phy_write(dev, 0x0493, 0x287A);
    944. 

  944. 		b43_phy_write(dev, 0x04AA, 0x1CA8);
    945. 

  945. 		b43_phy_write(dev, 0x04AC, 0x287A);
    946. 

  946. 

  947. 		b43_phy_maskset(dev, 0x04A0, 0xFFC0, 0x001A);
    948. 

  948. 		b43_phy_write(dev, 0x04A7, 0x000D);
    949. 

  949. 

  950. 		if (phy->rev < 2) {
    951. 

  951. 			b43_phy_write(dev, 0x0406, 0xFF0D);
    952. 

  952. 		} else if (phy->rev == 2) {
    953. 

  953. 			b43_phy_write(dev, 0x04C0, 0xFFFF);
    954. 

  954. 			b43_phy_write(dev, 0x04C1, 0x00A9);
    955. 

  955. 		} else {
    956. 

  956. 			b43_phy_write(dev, 0x04C0, 0x00C1);
    957. 

  957. 			b43_phy_write(dev, 0x04C1, 0x0059);
    958. 

  958. 		}
    959. 

  959. 

  960. 		b43_phy_maskset(dev, 0x04A1, 0xC0FF, 0x1800);
    961. 

  961. 		b43_phy_maskset(dev, 0x04A1, 0xFFC0, 0x0015);
    962. 

  962. 		b43_phy_maskset(dev, 0x04A8, 0xCFFF, 0x1000);
    963. 

  963. 		b43_phy_maskset(dev, 0x04A8, 0xF0FF, 0x0A00);
    964. 

  964. 		b43_phy_maskset(dev, 0x04AB, 0xCFFF, 0x1000);
    965. 

  965. 		b43_phy_maskset(dev, 0x04AB, 0xF0FF, 0x0800);
    966. 

  966. 		b43_phy_maskset(dev, 0x04AB, 0xFFCF, 0x0010);
    967. 

  967. 		b43_phy_maskset(dev, 0x04AB, 0xFFF0, 0x0005);
    968. 

  968. 		b43_phy_maskset(dev, 0x04A8, 0xFFCF, 0x0010);
    969. 

  969. 		b43_phy_maskset(dev, 0x04A8, 0xFFF0, 0x0006);
    970. 

  970. 		b43_phy_maskset(dev, 0x04A2, 0xF0FF, 0x0800);
    971. 

  971. 		b43_phy_maskset(dev, 0x04A0, 0xF0FF, 0x0500);
    972. 

  972. 		b43_phy_maskset(dev, 0x04A2, 0xFFF0, 0x000B);
    973. 

  973. 

  974. 		if (phy->rev >= 3) {
    975. 

  975. 			b43_phy_mask(dev, 0x048A, 0x7FFF);
    976. 

  976. 			b43_phy_maskset(dev, 0x0415, 0x8000, 0x36D8);
    977. 

  977. 			b43_phy_maskset(dev, 0x0416, 0x8000, 0x36D8);
    978. 

  978. 			b43_phy_maskset(dev, 0x0417, 0xFE00, 0x016D);
    979. 

  979. 		} else {
    980. 

  980. 			b43_phy_set(dev, 0x048A, 0x1000);
    981. 

  981. 			b43_phy_maskset(dev, 0x048A, 0x9FFF, 0x2000);
    982. 

  982. 			b43_hf_write(dev, b43_hf_read(dev) | B43_HF_ACIW);
    983. 

  983. 		}
    984. 

  984. 		if (phy->rev >= 2) {
    985. 

  985. 			b43_phy_set(dev, 0x042B, 0x0800);
    986. 

  986. 		}
    987. 

  987. 		b43_phy_maskset(dev, 0x048C, 0xF0FF, 0x0200);
    988. 

  988. 		if (phy->rev == 2) {
    989. 

  989. 			b43_phy_maskset(dev, 0x04AE, 0xFF00, 0x007F);
    990. 

  990. 			b43_phy_maskset(dev, 0x04AD, 0x00FF, 0x1300);
    991. 

  991. 		} else if (phy->rev >= 6) {
    992. 

  992. 			b43_ofdmtab_write16(dev, 0x1A00, 0x3, 0x007F);
    993. 

  993. 			b43_ofdmtab_write16(dev, 0x1A00, 0x2, 0x007F);
    994. 

  994. 			b43_phy_mask(dev, 0x04AD, 0x00FF);
    995. 

  995. 		}
    996. 

  996. 		b43_calc_nrssi_slope(dev);
    997. 

  997. 		break;
    998. 

  998. 	default:
    999. 

  999. 		B43_WARN_ON(1);
    1000. 

  1000. 	}
    1001. 

  1001. }
    1002. 

  1002. 

  1003. static void
    1004. 

  1004. b43_radio_interference_mitigation_disable(struct b43_wldev *dev, int mode)
    1005. 

  1005. {
    1006. 

  1006. 	struct b43_phy *phy = &dev->phy;
    1007. 

  1007. 	struct b43_phy_g *gphy = phy->g;
    1008. 

  1008. 	u32 *stack = gphy->interfstack;
    1009. 

  1009. 

  1010. 	switch (mode) {
    1011. 

  1011. 	case B43_INTERFMODE_NONWLAN:
    1012. 

  1012. 		if (phy->rev != 1) {
    1013. 

  1013. 			b43_phy_mask(dev, 0x042B, ~0x0800);
    1014. 

  1014. 			b43_phy_set(dev, B43_PHY_G_CRS, 0x4000);
    1015. 

  1015. 			break;
    1016. 

  1016. 		}
    1017. 

  1017. 		radio_stackrestore(0x0078);
    1018. 

  1018. 		b43_calc_nrssi_threshold(dev);
    1019. 

  1019. 		phy_stackrestore(0x0406);
    1020. 

  1020. 		b43_phy_mask(dev, 0x042B, ~0x0800);
    1021. 

  1021. 		if (!dev->bad_frames_preempt) {
    1022. 

  1022. 			b43_phy_mask(dev, B43_PHY_RADIO_BITFIELD, ~(1 << 11));
    1023. 

  1023. 		}
    1024. 

  1024. 		b43_phy_set(dev, B43_PHY_G_CRS, 0x4000);
    1025. 

  1025. 		phy_stackrestore(0x04A0);
    1026. 

  1026. 		phy_stackrestore(0x04A1);
    1027. 

  1027. 		phy_stackrestore(0x04A2);
    1028. 

  1028. 		phy_stackrestore(0x04A8);
    1029. 

  1029. 		phy_stackrestore(0x04AB);
    1030. 

  1030. 		phy_stackrestore(0x04A7);
    1031. 

  1031. 		phy_stackrestore(0x04A3);
    1032. 

  1032. 		phy_stackrestore(0x04A9);
    1033. 

  1033. 		phy_stackrestore(0x0493);
    1034. 

  1034. 		phy_stackrestore(0x04AA);
    1035. 

  1035. 		phy_stackrestore(0x04AC);
    1036. 

  1036. 		break;
    1037. 

  1037. 	case B43_INTERFMODE_MANUALWLAN:
    1038. 

  1038. 		if (!(b43_phy_read(dev, 0x0033) & 0x0800))
    1039. 

  1039. 			break;
    1040. 

  1040. 

  1041. 		gphy->aci_enable = 0;
    1042. 

  1042. 

  1043. 		phy_stackrestore(B43_PHY_RADIO_BITFIELD);
    1044. 

  1044. 		phy_stackrestore(B43_PHY_G_CRS);
    1045. 

  1045. 		phy_stackrestore(0x0033);
    1046. 

  1046. 		phy_stackrestore(0x04A3);
    1047. 

  1047. 		phy_stackrestore(0x04A9);
    1048. 

  1048. 		phy_stackrestore(0x0493);
    1049. 

  1049. 		phy_stackrestore(0x04AA);
    1050. 

  1050. 		phy_stackrestore(0x04AC);
    1051. 

  1051. 		phy_stackrestore(0x04A0);
    1052. 

  1052. 		phy_stackrestore(0x04A7);
    1053. 

  1053. 		if (phy->rev >= 2) {
    1054. 

  1054. 			phy_stackrestore(0x04C0);
    1055. 

  1055. 			phy_stackrestore(0x04C1);
    1056. 

  1056. 		} else
    1057. 

  1057. 			phy_stackrestore(0x0406);
    1058. 

  1058. 		phy_stackrestore(0x04A1);
    1059. 

  1059. 		phy_stackrestore(0x04AB);
    1060. 

  1060. 		phy_stackrestore(0x04A8);
    1061. 

  1061. 		if (phy->rev == 2) {
    1062. 

  1062. 			phy_stackrestore(0x04AD);
    1063. 

  1063. 			phy_stackrestore(0x04AE);
    1064. 

  1064. 		} else if (phy->rev >= 3) {
    1065. 

  1065. 			phy_stackrestore(0x04AD);
    1066. 

  1066. 			phy_stackrestore(0x0415);
    1067. 

  1067. 			phy_stackrestore(0x0416);
    1068. 

  1068. 			phy_stackrestore(0x0417);
    1069. 

  1069. 			ofdmtab_stackrestore(0x1A00, 0x2);
    1070. 

  1070. 			ofdmtab_stackrestore(0x1A00, 0x3);
    1071. 

  1071. 		}
    1072. 

  1072. 		phy_stackrestore(0x04A2);
    1073. 

  1073. 		phy_stackrestore(0x048A);
    1074. 

  1074. 		phy_stackrestore(0x042B);
    1075. 

  1075. 		phy_stackrestore(0x048C);
    1076. 

  1076. 		b43_hf_write(dev, b43_hf_read(dev) & ~B43_HF_ACIW);
    1077. 

  1077. 		b43_calc_nrssi_slope(dev);
    1078. 

  1078. 		break;
    1079. 

  1079. 	default:
    1080. 

  1080. 		B43_WARN_ON(1);
    1081. 

  1081. 	}
    1082. 

  1082. }
    1083. 

  1083. 

  1084. #undef phy_stacksave
    1085. 

  1085. #undef phy_stackrestore
    1086. 

  1086. #undef radio_stacksave
    1087. 

  1087. #undef radio_stackrestore
    1088. 

  1088. #undef ofdmtab_stacksave
    1089. 

  1089. #undef ofdmtab_stackrestore
    1090. 

  1090. 

  1091. static u16 b43_radio_core_calibration_value(struct b43_wldev *dev)
    1092. 

  1092. {
    1093. 

  1093. 	u16 reg, index, ret;
    1094. 

  1094. 

  1095. 	static const u8 rcc_table[] = {
    1096. 

  1096. 		0x02, 0x03, 0x01, 0x0F,
    1097. 

  1097. 		0x06, 0x07, 0x05, 0x0F,
    1098. 

  1098. 		0x0A, 0x0B, 0x09, 0x0F,
    1099. 

  1099. 		0x0E, 0x0F, 0x0D, 0x0F,
    1100. 

  1100. 	};
    1101. 

  1101. 

  1102. 	reg = b43_radio_read16(dev, 0x60);
    1103. 

  1103. 	index = (reg & 0x001E) >> 1;
    1104. 

  1104. 	ret = rcc_table[index] << 1;
    1105. 

  1105. 	ret |= (reg & 0x0001);
    1106. 

  1106. 	ret |= 0x0020;
    1107. 

  1107. 

  1108. 	return ret;
    1109. 

  1109. }
    1110. 

  1110. 

  1111. #define LPD(L, P, D)	(((L) << 2) | ((P) << 1) | ((D) << 0))
    1112. 

  1112. static u16 radio2050_rfover_val(struct b43_wldev *dev,
    1113. 

  1113. 				u16 phy_register, unsigned int lpd)
    1114. 

  1114. {
    1115. 

  1115. 	struct b43_phy *phy = &dev->phy;
    1116. 

  1116. 	struct b43_phy_g *gphy = phy->g;
    1117. 

  1117. 	struct ssb_sprom *sprom = &(dev->sdev->bus->sprom);
    1118. 

  1118. 

  1119. 	if (!phy->gmode)
    1120. 

  1120. 		return 0;
    1121. 

  1121. 

  1122. 	if (has_loopback_gain(phy)) {
    1123. 

  1123. 		int max_lb_gain = gphy->max_lb_gain;
    1124. 

  1124. 		u16 extlna;
    1125. 

  1125. 		u16 i;
    1126. 

  1126. 

  1127. 		if (phy->radio_rev == 8)
    1128. 

  1128. 			max_lb_gain += 0x3E;
    1129. 

  1129. 		else
    1130. 

  1130. 			max_lb_gain += 0x26;
    1131. 

  1131. 		if (max_lb_gain >= 0x46) {
    1132. 

  1132. 			extlna = 0x3000;
    1133. 

  1133. 			max_lb_gain -= 0x46;
    1134. 

  1134. 		} else if (max_lb_gain >= 0x3A) {
    1135. 

  1135. 			extlna = 0x1000;
    1136. 

  1136. 			max_lb_gain -= 0x3A;
    1137. 

  1137. 		} else if (max_lb_gain >= 0x2E) {
    1138. 

  1138. 			extlna = 0x2000;
    1139. 

  1139. 			max_lb_gain -= 0x2E;
    1140. 

  1140. 		} else {
    1141. 

  1141. 			extlna = 0;
    1142. 

  1142. 			max_lb_gain -= 0x10;
    1143. 

  1143. 		}
    1144. 

  1144. 

  1145. 		for (i = 0; i < 16; i++) {
    1146. 

  1146. 			max_lb_gain -= (i * 6);
    1147. 

  1147. 			if (max_lb_gain < 6)
    1148. 

  1148. 				break;
    1149. 

  1149. 		}
    1150. 

  1150. 

  1151. 		if ((phy->rev < 7) ||
    1152. 

  1152. 		    !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
    1153. 

  1153. 			if (phy_register == B43_PHY_RFOVER) {
    1154. 

  1154. 				return 0x1B3;
    1155. 

  1155. 			} else if (phy_register == B43_PHY_RFOVERVAL) {
    1156. 

  1156. 				extlna |= (i << 8);
    1157. 

  1157. 				switch (lpd) {
    1158. 

  1158. 				case LPD(0, 1, 1):
    1159. 

  1159. 					return 0x0F92;
    1160. 

  1160. 				case LPD(0, 0, 1):
    1161. 

  1161. 				case LPD(1, 0, 1):
    1162. 

  1162. 					return (0x0092 | extlna);
    1163. 

  1163. 				case LPD(1, 0, 0):
    1164. 

  1164. 					return (0x0093 | extlna);
    1165. 

  1165. 				}
    1166. 

  1166. 				B43_WARN_ON(1);
    1167. 

  1167. 			}
    1168. 

  1168. 			B43_WARN_ON(1);
    1169. 

  1169. 		} else {
    1170. 

  1170. 			if (phy_register == B43_PHY_RFOVER) {
    1171. 

  1171. 				return 0x9B3;
    1172. 

  1172. 			} else if (phy_register == B43_PHY_RFOVERVAL) {
    1173. 

  1173. 				if (extlna)
    1174. 

  1174. 					extlna |= 0x8000;
    1175. 

  1175. 				extlna |= (i << 8);
    1176. 

  1176. 				switch (lpd) {
    1177. 

  1177. 				case LPD(0, 1, 1):
    1178. 

  1178. 					return 0x8F92;
    1179. 

  1179. 				case LPD(0, 0, 1):
    1180. 

  1180. 					return (0x8092 | extlna);
    1181. 

  1181. 				case LPD(1, 0, 1):
    1182. 

  1182. 					return (0x2092 | extlna);
    1183. 

  1183. 				case LPD(1, 0, 0):
    1184. 

  1184. 					return (0x2093 | extlna);
    1185. 

  1185. 				}
    1186. 

  1186. 				B43_WARN_ON(1);
    1187. 

  1187. 			}
    1188. 

  1188. 			B43_WARN_ON(1);
    1189. 

  1189. 		}
    1190. 

  1190. 	} else {
    1191. 

  1191. 		if ((phy->rev < 7) ||
    1192. 

  1192. 		    !(sprom->boardflags_lo & B43_BFL_EXTLNA)) {
    1193. 

  1193. 			if (phy_register == B43_PHY_RFOVER) {
    1194. 

  1194. 				return 0x1B3;
    1195. 

  1195. 			} else if (phy_register == B43_PHY_RFOVERVAL) {
    1196. 

  1196. 				switch (lpd) {
    1197. 

  1197. 				case LPD(0, 1, 1):
    1198. 

  1198. 					return 0x0FB2;
    1199. 

  1199. 				case LPD(0, 0, 1):
    1200. 

  1200. 					return 0x00B2;
    1201. 

  1201. 				case LPD(1, 0, 1):
    1202. 

  1202. 					return 0x30B2;
    1203. 

  1203. 				case LPD(1, 0, 0):
    1204. 

  1204. 					return 0x30B3;
    1205. 

  1205. 				}
    1206. 

  1206. 				B43_WARN_ON(1);
    1207. 

  1207. 			}
    1208. 

  1208. 			B43_WARN_ON(1);
    1209. 

  1209. 		} else {
    1210. 

  1210. 			if (phy_register == B43_PHY_RFOVER) {
    1211. 

  1211. 				return 0x9B3;
    1212. 

  1212. 			} else if (phy_register == B43_PHY_RFOVERVAL) {
    1213. 

  1213. 				switch (lpd) {
    1214. 

  1214. 				case LPD(0, 1, 1):
    1215. 

  1215. 					return 0x8FB2;
    1216. 

  1216. 				case LPD(0, 0, 1):
    1217. 

  1217. 					return 0x80B2;
    1218. 

  1218. 				case LPD(1, 0, 1):
    1219. 

  1219. 					return 0x20B2;
    1220. 

  1220. 				case LPD(1, 0, 0):
    1221. 

  1221. 					return 0x20B3;
    1222. 

  1222. 				}
    1223. 

  1223. 				B43_WARN_ON(1);
    1224. 

  1224. 			}
    1225. 

  1225. 			B43_WARN_ON(1);
    1226. 

  1226. 		}
    1227. 

  1227. 	}
    1228. 

  1228. 	return 0;
    1229. 

  1229. }
    1230. 

  1230. 

  1231. struct init2050_saved_values {
    1232. 

  1232. 	/* Core registers */
    1233. 

  1233. 	u16 reg_3EC;
    1234. 

  1234. 	u16 reg_3E6;
    1235. 

  1235. 	u16 reg_3F4;
    1236. 

  1236. 	/* Radio registers */
    1237. 

  1237. 	u16 radio_43;
    1238. 

  1238. 	u16 radio_51;
    1239. 

  1239. 	u16 radio_52;
    1240. 

  1240. 	/* PHY registers */
    1241. 

  1241. 	u16 phy_pgactl;
    1242. 

  1242. 	u16 phy_cck_5A;
    1243. 

  1243. 	u16 phy_cck_59;
    1244. 

  1244. 	u16 phy_cck_58;
    1245. 

  1245. 	u16 phy_cck_30;
    1246. 

  1246. 	u16 phy_rfover;
    1247. 

  1247. 	u16 phy_rfoverval;
    1248. 

  1248. 	u16 phy_analogover;
    1249. 

  1249. 	u16 phy_analogoverval;
    1250. 

  1250. 	u16 phy_crs0;
    1251. 

  1251. 	u16 phy_classctl;
    1252. 

  1252. 	u16 phy_lo_mask;
    1253. 

  1253. 	u16 phy_lo_ctl;
    1254. 

  1254. 	u16 phy_syncctl;
    1255. 

  1255. };
    1256. 

  1256. 

  1257. static u16 b43_radio_init2050(struct b43_wldev *dev)
    1258. 

  1258. {
    1259. 

  1259. 	struct b43_phy *phy = &dev->phy;
    1260. 

  1260. 	struct init2050_saved_values sav;
    1261. 

  1261. 	u16 rcc;
    1262. 

  1262. 	u16 radio78;
    1263. 

  1263. 	u16 ret;
    1264. 

  1264. 	u16 i, j;
    1265. 

  1265. 	u32 tmp1 = 0, tmp2 = 0;
    1266. 

  1266. 

  1267. 	memset(&sav, 0, sizeof(sav));	/* get rid of "may be used uninitialized..." */
    1268. 

  1268. 

  1269. 	sav.radio_43 = b43_radio_read16(dev, 0x43);
    1270. 

  1270. 	sav.radio_51 = b43_radio_read16(dev, 0x51);
    1271. 

  1271. 	sav.radio_52 = b43_radio_read16(dev, 0x52);
    1272. 

  1272. 	sav.phy_pgactl = b43_phy_read(dev, B43_PHY_PGACTL);
    1273. 

  1273. 	sav.phy_cck_5A = b43_phy_read(dev, B43_PHY_CCK(0x5A));
    1274. 

  1274. 	sav.phy_cck_59 = b43_phy_read(dev, B43_PHY_CCK(0x59));
    1275. 

  1275. 	sav.phy_cck_58 = b43_phy_read(dev, B43_PHY_CCK(0x58));
    1276. 

  1276. 

  1277. 	if (phy->type == B43_PHYTYPE_B) {
    1278. 

  1278. 		sav.phy_cck_30 = b43_phy_read(dev, B43_PHY_CCK(0x30));
    1279. 

  1279. 		sav.reg_3EC = b43_read16(dev, 0x3EC);
    1280. 

  1280. 

  1281. 		b43_phy_write(dev, B43_PHY_CCK(0x30), 0xFF);
    1282. 

  1282. 		b43_write16(dev, 0x3EC, 0x3F3F);
    1283. 

  1283. 	} else if (phy->gmode || phy->rev >= 2) {
    1284. 

  1284. 		sav.phy_rfover = b43_phy_read(dev, B43_PHY_RFOVER);
    1285. 

  1285. 		sav.phy_rfoverval = b43_phy_read(dev, B43_PHY_RFOVERVAL);
    1286. 

  1286. 		sav.phy_analogover = b43_phy_read(dev, B43_PHY_ANALOGOVER);
    1287. 

  1287. 		sav.phy_analogoverval =
    1288. 

  1288. 		    b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
    1289. 

  1289. 		sav.phy_crs0 = b43_phy_read(dev, B43_PHY_CRS0);
    1290. 

  1290. 		sav.phy_classctl = b43_phy_read(dev, B43_PHY_CLASSCTL);
    1291. 

  1291. 

  1292. 		b43_phy_set(dev, B43_PHY_ANALOGOVER, 0x0003);
    1293. 

  1293. 		b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, 0xFFFC);
    1294. 

  1294. 		b43_phy_mask(dev, B43_PHY_CRS0, 0x7FFF);
    1295. 

  1295. 		b43_phy_mask(dev, B43_PHY_CLASSCTL, 0xFFFC);
    1296. 

  1296. 		if (has_loopback_gain(phy)) {
    1297. 

  1297. 			sav.phy_lo_mask = b43_phy_read(dev, B43_PHY_LO_MASK);
    1298. 

  1298. 			sav.phy_lo_ctl = b43_phy_read(dev, B43_PHY_LO_CTL);
    1299. 

  1299. 

  1300. 			if (phy->rev >= 3)
    1301. 

  1301. 				b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
    1302. 

  1302. 			else
    1303. 

  1303. 				b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
    1304. 

  1304. 			b43_phy_write(dev, B43_PHY_LO_CTL, 0);
    1305. 

  1305. 		}
    1306. 

  1306. 

  1307. 		b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1308. 

  1308. 			      radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
    1309. 

  1309. 						   LPD(0, 1, 1)));
    1310. 

  1310. 		b43_phy_write(dev, B43_PHY_RFOVER,
    1311. 

  1311. 			      radio2050_rfover_val(dev, B43_PHY_RFOVER, 0));
    1312. 

  1312. 	}
    1313. 

  1313. 	b43_write16(dev, 0x3E2, b43_read16(dev, 0x3E2) | 0x8000);
    1314. 

  1314. 

  1315. 	sav.phy_syncctl = b43_phy_read(dev, B43_PHY_SYNCCTL);
    1316. 

  1316. 	b43_phy_mask(dev, B43_PHY_SYNCCTL, 0xFF7F);
    1317. 

  1317. 	sav.reg_3E6 = b43_read16(dev, 0x3E6);
    1318. 

  1318. 	sav.reg_3F4 = b43_read16(dev, 0x3F4);
    1319. 

  1319. 

  1320. 	if (phy->analog == 0) {
    1321. 

  1321. 		b43_write16(dev, 0x03E6, 0x0122);
    1322. 

  1322. 	} else {
    1323. 

  1323. 		if (phy->analog >= 2) {
    1324. 

  1324. 			b43_phy_maskset(dev, B43_PHY_CCK(0x03), 0xFFBF, 0x40);
    1325. 

  1325. 		}
    1326. 

  1326. 		b43_write16(dev, B43_MMIO_CHANNEL_EXT,
    1327. 

  1327. 			    (b43_read16(dev, B43_MMIO_CHANNEL_EXT) | 0x2000));
    1328. 

  1328. 	}
    1329. 

  1329. 

  1330. 	rcc = b43_radio_core_calibration_value(dev);
    1331. 

  1331. 

  1332. 	if (phy->type == B43_PHYTYPE_B)
    1333. 

  1333. 		b43_radio_write16(dev, 0x78, 0x26);
    1334. 

  1334. 	if (phy->gmode || phy->rev >= 2) {
    1335. 

  1335. 		b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1336. 

  1336. 			      radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
    1337. 

  1337. 						   LPD(0, 1, 1)));
    1338. 

  1338. 	}
    1339. 

  1339. 	b43_phy_write(dev, B43_PHY_PGACTL, 0xBFAF);
    1340. 

  1340. 	b43_phy_write(dev, B43_PHY_CCK(0x2B), 0x1403);
    1341. 

  1341. 	if (phy->gmode || phy->rev >= 2) {
    1342. 

  1342. 		b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1343. 

  1343. 			      radio2050_rfover_val(dev, B43_PHY_RFOVERVAL,
    1344. 

  1344. 						   LPD(0, 0, 1)));
    1345. 

  1345. 	}
    1346. 

  1346. 	b43_phy_write(dev, B43_PHY_PGACTL, 0xBFA0);
    1347. 

  1347. 	b43_radio_set(dev, 0x51, 0x0004);
    1348. 

  1348. 	if (phy->radio_rev == 8) {
    1349. 

  1349. 		b43_radio_write16(dev, 0x43, 0x1F);
    1350. 

  1350. 	} else {
    1351. 

  1351. 		b43_radio_write16(dev, 0x52, 0);
    1352. 

  1352. 		b43_radio_maskset(dev, 0x43, 0xFFF0, 0x0009);
    1353. 

  1353. 	}
    1354. 

  1354. 	b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
    1355. 

  1355. 

  1356. 	for (i = 0; i < 16; i++) {
    1357. 

  1357. 		b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0480);
    1358. 

  1358. 		b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
    1359. 

  1359. 		b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
    1360. 

  1360. 		if (phy->gmode || phy->rev >= 2) {
    1361. 

  1361. 			b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1362. 

  1362. 				      radio2050_rfover_val(dev,
    1363. 

  1363. 							   B43_PHY_RFOVERVAL,
    1364. 

  1364. 							   LPD(1, 0, 1)));
    1365. 

  1365. 		}
    1366. 

  1366. 		b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
    1367. 

  1367. 		udelay(10);
    1368. 

  1368. 		if (phy->gmode || phy->rev >= 2) {
    1369. 

  1369. 			b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1370. 

  1370. 				      radio2050_rfover_val(dev,
    1371. 

  1371. 							   B43_PHY_RFOVERVAL,
    1372. 

  1372. 							   LPD(1, 0, 1)));
    1373. 

  1373. 		}
    1374. 

  1374. 		b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
    1375. 

  1375. 		udelay(10);
    1376. 

  1376. 		if (phy->gmode || phy->rev >= 2) {
    1377. 

  1377. 			b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1378. 

  1378. 				      radio2050_rfover_val(dev,
    1379. 

  1379. 							   B43_PHY_RFOVERVAL,
    1380. 

  1380. 							   LPD(1, 0, 0)));
    1381. 

  1381. 		}
    1382. 

  1382. 		b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
    1383. 

  1383. 		udelay(20);
    1384. 

  1384. 		tmp1 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
    1385. 

  1385. 		b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
    1386. 

  1386. 		if (phy->gmode || phy->rev >= 2) {
    1387. 

  1387. 			b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1388. 

  1388. 				      radio2050_rfover_val(dev,
    1389. 

  1389. 							   B43_PHY_RFOVERVAL,
    1390. 

  1390. 							   LPD(1, 0, 1)));
    1391. 

  1391. 		}
    1392. 

  1392. 		b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
    1393. 

  1393. 	}
    1394. 

  1394. 	udelay(10);
    1395. 

  1395. 

  1396. 	b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
    1397. 

  1397. 	tmp1++;
    1398. 

  1398. 	tmp1 >>= 9;
    1399. 

  1399. 

  1400. 	for (i = 0; i < 16; i++) {
    1401. 

  1401. 		radio78 = (bitrev4(i) << 1) | 0x0020;
    1402. 

  1402. 		b43_radio_write16(dev, 0x78, radio78);
    1403. 

  1403. 		udelay(10);
    1404. 

  1404. 		for (j = 0; j < 16; j++) {
    1405. 

  1405. 			b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0D80);
    1406. 

  1406. 			b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
    1407. 

  1407. 			b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
    1408. 

  1408. 			if (phy->gmode || phy->rev >= 2) {
    1409. 

  1409. 				b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1410. 

  1410. 					      radio2050_rfover_val(dev,
    1411. 

  1411. 								   B43_PHY_RFOVERVAL,
    1412. 

  1412. 								   LPD(1, 0,
    1413. 

  1413. 								       1)));
    1414. 

  1414. 			}
    1415. 

  1415. 			b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
    1416. 

  1416. 			udelay(10);
    1417. 

  1417. 			if (phy->gmode || phy->rev >= 2) {
    1418. 

  1418. 				b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1419. 

  1419. 					      radio2050_rfover_val(dev,
    1420. 

  1420. 								   B43_PHY_RFOVERVAL,
    1421. 

  1421. 								   LPD(1, 0,
    1422. 

  1422. 								       1)));
    1423. 

  1423. 			}
    1424. 

  1424. 			b43_phy_write(dev, B43_PHY_PGACTL, 0xEFB0);
    1425. 

  1425. 			udelay(10);
    1426. 

  1426. 			if (phy->gmode || phy->rev >= 2) {
    1427. 

  1427. 				b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1428. 

  1428. 					      radio2050_rfover_val(dev,
    1429. 

  1429. 								   B43_PHY_RFOVERVAL,
    1430. 

  1430. 								   LPD(1, 0,
    1431. 

  1431. 								       0)));
    1432. 

  1432. 			}
    1433. 

  1433. 			b43_phy_write(dev, B43_PHY_PGACTL, 0xFFF0);
    1434. 

  1434. 			udelay(10);
    1435. 

  1435. 			tmp2 += b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
    1436. 

  1436. 			b43_phy_write(dev, B43_PHY_CCK(0x58), 0);
    1437. 

  1437. 			if (phy->gmode || phy->rev >= 2) {
    1438. 

  1438. 				b43_phy_write(dev, B43_PHY_RFOVERVAL,
    1439. 

  1439. 					      radio2050_rfover_val(dev,
    1440. 

  1440. 								   B43_PHY_RFOVERVAL,
    1441. 

  1441. 								   LPD(1, 0,
    1442. 

  1442. 								       1)));
    1443. 

  1443. 			}
    1444. 

  1444. 			b43_phy_write(dev, B43_PHY_PGACTL, 0xAFB0);
    1445. 

  1445. 		}
    1446. 

  1446. 		tmp2++;
    1447. 

  1447. 		tmp2 >>= 8;
    1448. 

  1448. 		if (tmp1 < tmp2)
    1449. 

  1449. 			break;
    1450. 

  1450. 	}
    1451. 

  1451. 

  1452. 	/* Restore the registers */
    1453. 

  1453. 	b43_phy_write(dev, B43_PHY_PGACTL, sav.phy_pgactl);
    1454. 

  1454. 	b43_radio_write16(dev, 0x51, sav.radio_51);
    1455. 

  1455. 	b43_radio_write16(dev, 0x52, sav.radio_52);
    1456. 

  1456. 	b43_radio_write16(dev, 0x43, sav.radio_43);
    1457. 

  1457. 	b43_phy_write(dev, B43_PHY_CCK(0x5A), sav.phy_cck_5A);
    1458. 

  1458. 	b43_phy_write(dev, B43_PHY_CCK(0x59), sav.phy_cck_59);
    1459. 

  1459. 	b43_phy_write(dev, B43_PHY_CCK(0x58), sav.phy_cck_58);
    1460. 

  1460. 	b43_write16(dev, 0x3E6, sav.reg_3E6);
    1461. 

  1461. 	if (phy->analog != 0)
    1462. 

  1462. 		b43_write16(dev, 0x3F4, sav.reg_3F4);
    1463. 

  1463. 	b43_phy_write(dev, B43_PHY_SYNCCTL, sav.phy_syncctl);
    1464. 

  1464. 	b43_synth_pu_workaround(dev, phy->channel);
    1465. 

  1465. 	if (phy->type == B43_PHYTYPE_B) {
    1466. 

  1466. 		b43_phy_write(dev, B43_PHY_CCK(0x30), sav.phy_cck_30);
    1467. 

  1467. 		b43_write16(dev, 0x3EC, sav.reg_3EC);
    1468. 

  1468. 	} else if (phy->gmode) {
    1469. 

  1469. 		b43_write16(dev, B43_MMIO_PHY_RADIO,
    1470. 

  1470. 			    b43_read16(dev, B43_MMIO_PHY_RADIO)
    1471. 

  1471. 			    & 0x7FFF);
    1472. 

  1472. 		b43_phy_write(dev, B43_PHY_RFOVER, sav.phy_rfover);
    1473. 

  1473. 		b43_phy_write(dev, B43_PHY_RFOVERVAL, sav.phy_rfoverval);
    1474. 

  1474. 		b43_phy_write(dev, B43_PHY_ANALOGOVER, sav.phy_analogover);
    1475. 

  1475. 		b43_phy_write(dev, B43_PHY_ANALOGOVERVAL,
    1476. 

  1476. 			      sav.phy_analogoverval);
    1477. 

  1477. 		b43_phy_write(dev, B43_PHY_CRS0, sav.phy_crs0);
    1478. 

  1478. 		b43_phy_write(dev, B43_PHY_CLASSCTL, sav.phy_classctl);
    1479. 

  1479. 		if (has_loopback_gain(phy)) {
    1480. 

  1480. 			b43_phy_write(dev, B43_PHY_LO_MASK, sav.phy_lo_mask);
    1481. 

  1481. 			b43_phy_write(dev, B43_PHY_LO_CTL, sav.phy_lo_ctl);
    1482. 

  1482. 		}
    1483. 

  1483. 	}
    1484. 

  1484. 	if (i > 15)
    1485. 

  1485. 		ret = radio78;
    1486. 

  1486. 	else
    1487. 

  1487. 		ret = rcc;
    1488. 

  1488. 

  1489. 	return ret;
    1490. 

  1490. }
    1491. 

  1491. 

  1492. static void b43_phy_initb5(struct b43_wldev *dev)
    1493. 

  1493. {
    1494. 

  1494. 	struct ssb_bus *bus = dev->sdev->bus;
    1495. 

  1495. 	struct b43_phy *phy = &dev->phy;
    1496. 

  1496. 	struct b43_phy_g *gphy = phy->g;
    1497. 

  1497. 	u16 offset, value;
    1498. 

  1498. 	u8 old_channel;
    1499. 

  1499. 

  1500. 	if (phy->analog == 1) {
    1501. 

  1501. 		b43_radio_set(dev, 0x007A, 0x0050);
    1502. 

  1502. 	}
    1503. 

  1503. 	if ((bus->boardinfo.vendor != SSB_BOARDVENDOR_BCM) &&
    1504. 

  1504. 	    (bus->boardinfo.type != SSB_BOARD_BU4306)) {
    1505. 

  1505. 		value = 0x2120;
    1506. 

  1506. 		for (offset = 0x00A8; offset < 0x00C7; offset++) {
    1507. 

  1507. 			b43_phy_write(dev, offset, value);
    1508. 

  1508. 			value += 0x202;
    1509. 

  1509. 		}
    1510. 

  1510. 	}
    1511. 

  1511. 	b43_phy_maskset(dev, 0x0035, 0xF0FF, 0x0700);
    1512. 

  1512. 	if (phy->radio_ver == 0x2050)
    1513. 

  1513. 		b43_phy_write(dev, 0x0038, 0x0667);
    1514. 

  1514. 

  1515. 	if (phy->gmode || phy->rev >= 2) {
    1516. 

  1516. 		if (phy->radio_ver == 0x2050) {
    1517. 

  1517. 			b43_radio_set(dev, 0x007A, 0x0020);
    1518. 

  1518. 			b43_radio_set(dev, 0x0051, 0x0004);
    1519. 

  1519. 		}
    1520. 

  1520. 		b43_write16(dev, B43_MMIO_PHY_RADIO, 0x0000);
    1521. 

  1521. 

  1522. 		b43_phy_set(dev, 0x0802, 0x0100);
    1523. 

  1523. 		b43_phy_set(dev, 0x042B, 0x2000);
    1524. 

  1524. 

  1525. 		b43_phy_write(dev, 0x001C, 0x186A);
    1526. 

  1526. 

  1527. 		b43_phy_maskset(dev, 0x0013, 0x00FF, 0x1900);
    1528. 

  1528. 		b43_phy_maskset(dev, 0x0035, 0xFFC0, 0x0064);
    1529. 

  1529. 		b43_phy_maskset(dev, 0x005D, 0xFF80, 0x000A);
    1530. 

  1530. 	}
    1531. 

  1531. 

  1532. 	if (dev->bad_frames_preempt) {
    1533. 

  1533. 		b43_phy_set(dev, B43_PHY_RADIO_BITFIELD, (1 << 11));
    1534. 

  1534. 	}
    1535. 

  1535. 

  1536. 	if (phy->analog == 1) {
    1537. 

  1537. 		b43_phy_write(dev, 0x0026, 0xCE00);
    1538. 

  1538. 		b43_phy_write(dev, 0x0021, 0x3763);
    1539. 

  1539. 		b43_phy_write(dev, 0x0022, 0x1BC3);
    1540. 

  1540. 		b43_phy_write(dev, 0x0023, 0x06F9);
    1541. 

  1541. 		b43_phy_write(dev, 0x0024, 0x037E);
    1542. 

  1542. 	} else
    1543. 

  1543. 		b43_phy_write(dev, 0x0026, 0xCC00);
    1544. 

  1544. 	b43_phy_write(dev, 0x0030, 0x00C6);
    1545. 

  1545. 	b43_write16(dev, 0x03EC, 0x3F22);
    1546. 

  1546. 

  1547. 	if (phy->analog == 1)
    1548. 

  1548. 		b43_phy_write(dev, 0x0020, 0x3E1C);
    1549. 

  1549. 	else
    1550. 

  1550. 		b43_phy_write(dev, 0x0020, 0x301C);
    1551. 

  1551. 

  1552. 	if (phy->analog == 0)
    1553. 

  1553. 		b43_write16(dev, 0x03E4, 0x3000);
    1554. 

  1554. 

  1555. 	old_channel = phy->channel;
    1556. 

  1556. 	/* Force to channel 7, even if not supported. */
    1557. 

  1557. 	b43_gphy_channel_switch(dev, 7, 0);
    1558. 

  1558. 

  1559. 	if (phy->radio_ver != 0x2050) {
    1560. 

  1560. 		b43_radio_write16(dev, 0x0075, 0x0080);
    1561. 

  1561. 		b43_radio_write16(dev, 0x0079, 0x0081);
    1562. 

  1562. 	}
    1563. 

  1563. 

  1564. 	b43_radio_write16(dev, 0x0050, 0x0020);
    1565. 

  1565. 	b43_radio_write16(dev, 0x0050, 0x0023);
    1566. 

  1566. 

  1567. 	if (phy->radio_ver == 0x2050) {
    1568. 

  1568. 		b43_radio_write16(dev, 0x0050, 0x0020);
    1569. 

  1569. 		b43_radio_write16(dev, 0x005A, 0x0070);
    1570. 

  1570. 	}
    1571. 

  1571. 

  1572. 	b43_radio_write16(dev, 0x005B, 0x007B);
    1573. 

  1573. 	b43_radio_write16(dev, 0x005C, 0x00B0);
    1574. 

  1574. 

  1575. 	b43_radio_set(dev, 0x007A, 0x0007);
    1576. 

  1576. 

  1577. 	b43_gphy_channel_switch(dev, old_channel, 0);
    1578. 

  1578. 

  1579. 	b43_phy_write(dev, 0x0014, 0x0080);
    1580. 

  1580. 	b43_phy_write(dev, 0x0032, 0x00CA);
    1581. 

  1581. 	b43_phy_write(dev, 0x002A, 0x88A3);
    1582. 

  1582. 

  1583. 	b43_set_txpower_g(dev, &gphy->bbatt, &gphy->rfatt, gphy->tx_control);
    1584. 

  1584. 

  1585. 	if (phy->radio_ver == 0x2050)
    1586. 

  1586. 		b43_radio_write16(dev, 0x005D, 0x000D);
    1587. 

  1587. 

  1588. 	b43_write16(dev, 0x03E4, (b43_read16(dev, 0x03E4) & 0xFFC0) | 0x0004);
    1589. 

  1589. }
    1590. 

  1590. 

  1591. static void b43_phy_initb6(struct b43_wldev *dev)
    1592. 

  1592. {
    1593. 

  1593. 	struct b43_phy *phy = &dev->phy;
    1594. 

  1594. 	struct b43_phy_g *gphy = phy->g;
    1595. 

  1595. 	u16 offset, val;
    1596. 

  1596. 	u8 old_channel;
    1597. 

  1597. 

  1598. 	b43_phy_write(dev, 0x003E, 0x817A);
    1599. 

  1599. 	b43_radio_write16(dev, 0x007A,
    1600. 

  1600. 			  (b43_radio_read16(dev, 0x007A) | 0x0058));
    1601. 

  1601. 	if (phy->radio_rev == 4 || phy->radio_rev == 5) {
    1602. 

  1602. 		b43_radio_write16(dev, 0x51, 0x37);
    1603. 

  1603. 		b43_radio_write16(dev, 0x52, 0x70);
    1604. 

  1604. 		b43_radio_write16(dev, 0x53, 0xB3);
    1605. 

  1605. 		b43_radio_write16(dev, 0x54, 0x9B);
    1606. 

  1606. 		b43_radio_write16(dev, 0x5A, 0x88);
    1607. 

  1607. 		b43_radio_write16(dev, 0x5B, 0x88);
    1608. 

  1608. 		b43_radio_write16(dev, 0x5D, 0x88);
    1609. 

  1609. 		b43_radio_write16(dev, 0x5E, 0x88);
    1610. 

  1610. 		b43_radio_write16(dev, 0x7D, 0x88);
    1611. 

  1611. 		b43_hf_write(dev, b43_hf_read(dev)
    1612. 

  1612. 			     | B43_HF_TSSIRPSMW);
    1613. 

  1613. 	}
    1614. 

  1614. 	B43_WARN_ON(phy->radio_rev == 6 || phy->radio_rev == 7);	/* We had code for these revs here... */
    1615. 

  1615. 	if (phy->radio_rev == 8) {
    1616. 

  1616. 		b43_radio_write16(dev, 0x51, 0);
    1617. 

  1617. 		b43_radio_write16(dev, 0x52, 0x40);
    1618. 

  1618. 		b43_radio_write16(dev, 0x53, 0xB7);
    1619. 

  1619. 		b43_radio_write16(dev, 0x54, 0x98);
    1620. 

  1620. 		b43_radio_write16(dev, 0x5A, 0x88);
    1621. 

  1621. 		b43_radio_write16(dev, 0x5B, 0x6B);
    1622. 

  1622. 		b43_radio_write16(dev, 0x5C, 0x0F);
    1623. 

  1623. 		if (dev->sdev->bus->sprom.boardflags_lo & B43_BFL_ALTIQ) {
    1624. 

  1624. 			b43_radio_write16(dev, 0x5D, 0xFA);
    1625. 

  1625. 			b43_radio_write16(dev, 0x5E, 0xD8);
    1626. 

  1626. 		} else {
    1627. 

  1627. 			b43_radio_write16(dev, 0x5D, 0xF5);
    1628. 

  1628. 			b43_radio_write16(dev, 0x5E, 0xB8);
    1629. 

  1629. 		}
    1630. 

  1630. 		b43_radio_write16(dev, 0x0073, 0x0003);
    1631. 

  1631. 		b43_radio_write16(dev, 0x007D, 0x00A8);
    1632. 

  1632. 		b43_radio_write16(dev, 0x007C, 0x0001);
    1633. 

  1633. 		b43_radio_write16(dev, 0x007E, 0x0008);
    1634. 

  1634. 	}
    1635. 

  1635. 	val = 0x1E1F;
    1636. 

  1636. 	for (offset = 0x0088; offset < 0x0098; offset++) {
    1637. 

  1637. 		b43_phy_write(dev, offset, val);
    1638. 

  1638. 		val -= 0x0202;
    1639. 

  1639. 	}
    1640. 

  1640. 	val = 0x3E3F;
    1641. 

  1641. 	for (offset = 0x0098; offset < 0x00A8; offset++) {
    1642. 

  1642. 		b43_phy_write(dev, offset, val);
    1643. 

  1643. 		val -= 0x0202;
    1644. 

  1644. 	}
    1645. 

  1645. 	val = 0x2120;
    1646. 

  1646. 	for (offset = 0x00A8; offset < 0x00C8; offset++) {
    1647. 

  1647. 		b43_phy_write(dev, offset, (val & 0x3F3F));
    1648. 

  1648. 		val += 0x0202;
    1649. 

  1649. 	}
    1650. 

  1650. 	if (phy->type == B43_PHYTYPE_G) {
    1651. 

  1651. 		b43_radio_set(dev, 0x007A, 0x0020);
    1652. 

  1652. 		b43_radio_set(dev, 0x0051, 0x0004);
    1653. 

  1653. 		b43_phy_set(dev, 0x0802, 0x0100);
    1654. 

  1654. 		b43_phy_set(dev, 0x042B, 0x2000);
    1655. 

  1655. 		b43_phy_write(dev, 0x5B, 0);
    1656. 

  1656. 		b43_phy_write(dev, 0x5C, 0);
    1657. 

  1657. 	}
    1658. 

  1658. 

  1659. 	old_channel = phy->channel;
    1660. 

  1660. 	if (old_channel >= 8)
    1661. 

  1661. 		b43_gphy_channel_switch(dev, 1, 0);
    1662. 

  1662. 	else
    1663. 

  1663. 		b43_gphy_channel_switch(dev, 13, 0);
    1664. 

  1664. 

  1665. 	b43_radio_write16(dev, 0x0050, 0x0020);
    1666. 

  1666. 	b43_radio_write16(dev, 0x0050, 0x0023);
    1667. 

  1667. 	udelay(40);
    1668. 

  1668. 	if (phy->radio_rev < 6 || phy->radio_rev == 8) {
    1669. 

  1669. 		b43_radio_write16(dev, 0x7C, (b43_radio_read16(dev, 0x7C)
    1670. 

  1670. 					      | 0x0002));
    1671. 

  1671. 		b43_radio_write16(dev, 0x50, 0x20);
    1672. 

  1672. 	}
    1673. 

  1673. 	if (phy->radio_rev <= 2) {
    1674. 

  1674. 		b43_radio_write16(dev, 0x7C, 0x20);
    1675. 

  1675. 		b43_radio_write16(dev, 0x5A, 0x70);
    1676. 

  1676. 		b43_radio_write16(dev, 0x5B, 0x7B);
    1677. 

  1677. 		b43_radio_write16(dev, 0x5C, 0xB0);
    1678. 

  1678. 	}
    1679. 

  1679. 	b43_radio_maskset(dev, 0x007A, 0x00F8, 0x0007);
    1680. 

  1680. 

  1681. 	b43_gphy_channel_switch(dev, old_channel, 0);
    1682. 

  1682. 

  1683. 	b43_phy_write(dev, 0x0014, 0x0200);
    1684. 

  1684. 	if (phy->radio_rev >= 6)
    1685. 

  1685. 		b43_phy_write(dev, 0x2A, 0x88C2);
    1686. 

  1686. 	else
    1687. 

  1687. 		b43_phy_write(dev, 0x2A, 0x8AC0);
    1688. 

  1688. 	b43_phy_write(dev, 0x0038, 0x0668);
    1689. 

  1689. 	b43_set_txpower_g(dev, &gphy->bbatt, &gphy->rfatt, gphy->tx_control);
    1690. 

  1690. 	if (phy->radio_rev <= 5) {
    1691. 

  1691. 		b43_phy_maskset(dev, 0x5D, 0xFF80, 0x0003);
    1692. 

  1692. 	}
    1693. 

  1693. 	if (phy->radio_rev <= 2)
    1694. 

  1694. 		b43_radio_write16(dev, 0x005D, 0x000D);
    1695. 

  1695. 

  1696. 	if (phy->analog == 4) {
    1697. 

  1697. 		b43_write16(dev, 0x3E4, 9);
    1698. 

  1698. 		b43_phy_mask(dev, 0x61, 0x0FFF);
    1699. 

  1699. 	} else {
    1700. 

  1700. 		b43_phy_maskset(dev, 0x0002, 0xFFC0, 0x0004);
    1701. 

  1701. 	}
    1702. 

  1702. 	if (phy->type == B43_PHYTYPE_B)
    1703. 

  1703. 		B43_WARN_ON(1);
    1704. 

  1704. 	else if (phy->type == B43_PHYTYPE_G)
    1705. 

  1705. 		b43_write16(dev, 0x03E6, 0x0);
    1706. 

  1706. }
    1707. 

  1707. 

  1708. static void b43_calc_loopback_gain(struct b43_wldev *dev)
    1709. 

  1709. {
    1710. 

  1710. 	struct b43_phy *phy = &dev->phy;
    1711. 

  1711. 	struct b43_phy_g *gphy = phy->g;
    1712. 

  1712. 	u16 backup_phy[16] = { 0 };
    1713. 

  1713. 	u16 backup_radio[3];
    1714. 

  1714. 	u16 backup_bband;
    1715. 

  1715. 	u16 i, j, loop_i_max;
    1716. 

  1716. 	u16 trsw_rx;
    1717. 

  1717. 	u16 loop1_outer_done, loop1_inner_done;
    1718. 

  1718. 

  1719. 	backup_phy[0] = b43_phy_read(dev, B43_PHY_CRS0);
    1720. 

  1720. 	backup_phy[1] = b43_phy_read(dev, B43_PHY_CCKBBANDCFG);
    1721. 

  1721. 	backup_phy[2] = b43_phy_read(dev, B43_PHY_RFOVER);
    1722. 

  1722. 	backup_phy[3] = b43_phy_read(dev, B43_PHY_RFOVERVAL);
    1723. 

  1723. 	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
    1724. 

  1724. 		backup_phy[4] = b43_phy_read(dev, B43_PHY_ANALOGOVER);
    1725. 

  1725. 		backup_phy[5] = b43_phy_read(dev, B43_PHY_ANALOGOVERVAL);
    1726. 

  1726. 	}
    1727. 

  1727. 	backup_phy[6] = b43_phy_read(dev, B43_PHY_CCK(0x5A));
    1728. 

  1728. 	backup_phy[7] = b43_phy_read(dev, B43_PHY_CCK(0x59));
    1729. 

  1729. 	backup_phy[8] = b43_phy_read(dev, B43_PHY_CCK(0x58));
    1730. 

  1730. 	backup_phy[9] = b43_phy_read(dev, B43_PHY_CCK(0x0A));
    1731. 

  1731. 	backup_phy[10] = b43_phy_read(dev, B43_PHY_CCK(0x03));
    1732. 

  1732. 	backup_phy[11] = b43_phy_read(dev, B43_PHY_LO_MASK);
    1733. 

  1733. 	backup_phy[12] = b43_phy_read(dev, B43_PHY_LO_CTL);
    1734. 

  1734. 	backup_phy[13] = b43_phy_read(dev, B43_PHY_CCK(0x2B));
    1735. 

  1735. 	backup_phy[14] = b43_phy_read(dev, B43_PHY_PGACTL);
    1736. 

  1736. 	backup_phy[15] = b43_phy_read(dev, B43_PHY_LO_LEAKAGE);
    1737. 

  1737. 	backup_bband = gphy->bbatt.att;
    1738. 

  1738. 	backup_radio[0] = b43_radio_read16(dev, 0x52);
    1739. 

  1739. 	backup_radio[1] = b43_radio_read16(dev, 0x43);
    1740. 

  1740. 	backup_radio[2] = b43_radio_read16(dev, 0x7A);
    1741. 

  1741. 

  1742. 	b43_phy_mask(dev, B43_PHY_CRS0, 0x3FFF);
    1743. 

  1743. 	b43_phy_set(dev, B43_PHY_CCKBBANDCFG, 0x8000);
    1744. 

  1744. 	b43_phy_set(dev, B43_PHY_RFOVER, 0x0002);
    1745. 

  1745. 	b43_phy_mask(dev, B43_PHY_RFOVERVAL, 0xFFFD);
    1746. 

  1746. 	b43_phy_set(dev, B43_PHY_RFOVER, 0x0001);
    1747. 

  1747. 	b43_phy_mask(dev, B43_PHY_RFOVERVAL, 0xFFFE);
    1748. 

  1748. 	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
    1749. 

  1749. 		b43_phy_set(dev, B43_PHY_ANALOGOVER, 0x0001);
    1750. 

  1750. 		b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, 0xFFFE);
    1751. 

  1751. 		b43_phy_set(dev, B43_PHY_ANALOGOVER, 0x0002);
    1752. 

  1752. 		b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, 0xFFFD);
    1753. 

  1753. 	}
    1754. 

  1754. 	b43_phy_set(dev, B43_PHY_RFOVER, 0x000C);
    1755. 

  1755. 	b43_phy_set(dev, B43_PHY_RFOVERVAL, 0x000C);
    1756. 

  1756. 	b43_phy_set(dev, B43_PHY_RFOVER, 0x0030);
    1757. 

  1757. 	b43_phy_maskset(dev, B43_PHY_RFOVERVAL, 0xFFCF, 0x10);
    1758. 

  1758. 

  1759. 	b43_phy_write(dev, B43_PHY_CCK(0x5A), 0x0780);
    1760. 

  1760. 	b43_phy_write(dev, B43_PHY_CCK(0x59), 0xC810);
    1761. 

  1761. 	b43_phy_write(dev, B43_PHY_CCK(0x58), 0x000D);
    1762. 

  1762. 

  1763. 	b43_phy_set(dev, B43_PHY_CCK(0x0A), 0x2000);
    1764. 

  1764. 	if (phy->rev != 1) {	/* Not in specs, but needed to prevent PPC machine check */
    1765. 

  1765. 		b43_phy_set(dev, B43_PHY_ANALOGOVER, 0x0004);
    1766. 

  1766. 		b43_phy_mask(dev, B43_PHY_ANALOGOVERVAL, 0xFFFB);
    1767. 

  1767. 	}
    1768. 

  1768. 	b43_phy_maskset(dev, B43_PHY_CCK(0x03), 0xFF9F, 0x40);
    1769. 

  1769. 

  1770. 	if (phy->radio_rev == 8) {
    1771. 

  1771. 		b43_radio_write16(dev, 0x43, 0x000F);
    1772. 

  1772. 	} else {
    1773. 

  1773. 		b43_radio_write16(dev, 0x52, 0);
    1774. 

  1774. 		b43_radio_maskset(dev, 0x43, 0xFFF0, 0x9);
    1775. 

  1775. 	}
    1776. 

  1776. 	b43_gphy_set_baseband_attenuation(dev, 11);
    1777. 

  1777. 

  1778. 	if (phy->rev >= 3)
    1779. 

  1779. 		b43_phy_write(dev, B43_PHY_LO_MASK, 0xC020);
    1780. 

  1780. 	else
    1781. 

  1781. 		b43_phy_write(dev, B43_PHY_LO_MASK, 0x8020);
    1782. 

  1782. 	b43_phy_write(dev, B43_PHY_LO_CTL, 0);
    1783. 

  1783. 

  1784. 	b43_phy_maskset(dev, B43_PHY_CCK(0x2B), 0xFFC0, 0x01);
    1785. 

  1785. 	b43_phy_maskset(dev, B43_PHY_CCK(0x2B), 0xC0FF, 0x800);
    1786. 

  1786. 

  1787. 	b43_phy_set(dev, B43_PHY_RFOVER, 0x0100);
    1788. 

  1788. 	b43_phy_mask(dev, B43_PHY_RFOVERVAL, 0xCFFF);
    1789. 

  1789. 

  1790. 	if (dev->sdev->bus…
    1791.