radio.c

linux内核源码

	if (phy->rev >= 6) {
		b43legacy_phy_write(dev, 0x0801, backup[16]);
		b43legacy_phy_write(dev, 0x0060, backup[17]);
		b43legacy_phy_write(dev, 0x0014, backup[18]);
		b43legacy_phy_write(dev, 0x0478, backup[19]);
	}
	b43legacy_phy_write(dev, 0x0001, backup[0]);
	b43legacy_phy_write(dev, 0x0812, backup[2]);
	b43legacy_phy_write(dev, 0x0811, backup[1]);
}

void b43legacy_calc_nrssi_slope(struct b43legacy_wldev *dev)
{
	struct b43legacy_phy *phy = &dev->phy;
	u16 backup[18] = { 0 };
	u16 tmp;
	s16 nrssi0;
	s16 nrssi1;

	switch (phy->type) {
	case B43legacy_PHYTYPE_B:
		backup[0] = b43legacy_radio_read16(dev, 0x007A);
		backup[1] = b43legacy_radio_read16(dev, 0x0052);
		backup[2] = b43legacy_radio_read16(dev, 0x0043);
		backup[3] = b43legacy_phy_read(dev, 0x0030);
		backup[4] = b43legacy_phy_read(dev, 0x0026);
		backup[5] = b43legacy_phy_read(dev, 0x0015);
		backup[6] = b43legacy_phy_read(dev, 0x002A);
		backup[7] = b43legacy_phy_read(dev, 0x0020);
		backup[8] = b43legacy_phy_read(dev, 0x005A);
		backup[9] = b43legacy_phy_read(dev, 0x0059);
		backup[10] = b43legacy_phy_read(dev, 0x0058);
		backup[11] = b43legacy_read16(dev, 0x03E2);
		backup[12] = b43legacy_read16(dev, 0x03E6);
		backup[13] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);

		tmp  = b43legacy_radio_read16(dev, 0x007A);
		tmp &= (phy->rev >= 5) ? 0x007F : 0x000F;
		b43legacy_radio_write16(dev, 0x007A, tmp);
		b43legacy_phy_write(dev, 0x0030, 0x00FF);
		b43legacy_write16(dev, 0x03EC, 0x7F7F);
		b43legacy_phy_write(dev, 0x0026, 0x0000);
		b43legacy_phy_write(dev, 0x0015,
				    b43legacy_phy_read(dev, 0x0015) | 0x0020);
		b43legacy_phy_write(dev, 0x002A, 0x08A3);
		b43legacy_radio_write16(dev, 0x007A,
					b43legacy_radio_read16(dev, 0x007A)
					| 0x0080);

		nrssi0 = (s16)b43legacy_phy_read(dev, 0x0027);
		b43legacy_radio_write16(dev, 0x007A,
					b43legacy_radio_read16(dev, 0x007A)
					& 0x007F);
		if (phy->analog >= 2)
			b43legacy_write16(dev, 0x03E6, 0x0040);
		else if (phy->analog == 0)
			b43legacy_write16(dev, 0x03E6, 0x0122);
		else
			b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
					  b43legacy_read16(dev,
					  B43legacy_MMIO_CHANNEL_EXT) & 0x2000);
		b43legacy_phy_write(dev, 0x0020, 0x3F3F);
		b43legacy_phy_write(dev, 0x0015, 0xF330);
		b43legacy_radio_write16(dev, 0x005A, 0x0060);
		b43legacy_radio_write16(dev, 0x0043,
					b43legacy_radio_read16(dev, 0x0043)
					& 0x00F0);
		b43legacy_phy_write(dev, 0x005A, 0x0480);
		b43legacy_phy_write(dev, 0x0059, 0x0810);
		b43legacy_phy_write(dev, 0x0058, 0x000D);
		udelay(20);

		nrssi1 = (s16)b43legacy_phy_read(dev, 0x0027);
		b43legacy_phy_write(dev, 0x0030, backup[3]);
		b43legacy_radio_write16(dev, 0x007A, backup[0]);
		b43legacy_write16(dev, 0x03E2, backup[11]);
		b43legacy_phy_write(dev, 0x0026, backup[4]);
		b43legacy_phy_write(dev, 0x0015, backup[5]);
		b43legacy_phy_write(dev, 0x002A, backup[6]);
		b43legacy_synth_pu_workaround(dev, phy->channel);
		if (phy->analog != 0)
			b43legacy_write16(dev, 0x03F4, backup[13]);

		b43legacy_phy_write(dev, 0x0020, backup[7]);
		b43legacy_phy_write(dev, 0x005A, backup[8]);
		b43legacy_phy_write(dev, 0x0059, backup[9]);
		b43legacy_phy_write(dev, 0x0058, backup[10]);
		b43legacy_radio_write16(dev, 0x0052, backup[1]);
		b43legacy_radio_write16(dev, 0x0043, backup[2]);

		if (nrssi0 == nrssi1)
			phy->nrssislope = 0x00010000;
		else
			phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);

		if (nrssi0 <= -4) {
			phy->nrssi[0] = nrssi0;
			phy->nrssi[1] = nrssi1;
		}
		break;
	case B43legacy_PHYTYPE_G:
		if (phy->radio_rev >= 9)
			return;
		if (phy->radio_rev == 8)
			b43legacy_calc_nrssi_offset(dev);

		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
				    b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
				    & 0x7FFF);
		b43legacy_phy_write(dev, 0x0802,
				    b43legacy_phy_read(dev, 0x0802) & 0xFFFC);
		backup[7] = b43legacy_read16(dev, 0x03E2);
		b43legacy_write16(dev, 0x03E2,
				  b43legacy_read16(dev, 0x03E2) | 0x8000);
		backup[0] = b43legacy_radio_read16(dev, 0x007A);
		backup[1] = b43legacy_radio_read16(dev, 0x0052);
		backup[2] = b43legacy_radio_read16(dev, 0x0043);
		backup[3] = b43legacy_phy_read(dev, 0x0015);
		backup[4] = b43legacy_phy_read(dev, 0x005A);
		backup[5] = b43legacy_phy_read(dev, 0x0059);
		backup[6] = b43legacy_phy_read(dev, 0x0058);
		backup[8] = b43legacy_read16(dev, 0x03E6);
		backup[9] = b43legacy_read16(dev, B43legacy_MMIO_CHANNEL_EXT);
		if (phy->rev >= 3) {
			backup[10] = b43legacy_phy_read(dev, 0x002E);
			backup[11] = b43legacy_phy_read(dev, 0x002F);
			backup[12] = b43legacy_phy_read(dev, 0x080F);
			backup[13] = b43legacy_phy_read(dev,
						B43legacy_PHY_G_LO_CONTROL);
			backup[14] = b43legacy_phy_read(dev, 0x0801);
			backup[15] = b43legacy_phy_read(dev, 0x0060);
			backup[16] = b43legacy_phy_read(dev, 0x0014);
			backup[17] = b43legacy_phy_read(dev, 0x0478);
			b43legacy_phy_write(dev, 0x002E, 0);
			b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL, 0);
			switch (phy->rev) {
			case 4: case 6: case 7:
				b43legacy_phy_write(dev, 0x0478,
						    b43legacy_phy_read(dev,
						    0x0478) | 0x0100);
				b43legacy_phy_write(dev, 0x0801,
						    b43legacy_phy_read(dev,
						    0x0801) | 0x0040);
				break;
			case 3: case 5:
				b43legacy_phy_write(dev, 0x0801,
						    b43legacy_phy_read(dev,
						    0x0801) & 0xFFBF);
				break;
			}
			b43legacy_phy_write(dev, 0x0060,
					    b43legacy_phy_read(dev, 0x0060)
					    | 0x0040);
			b43legacy_phy_write(dev, 0x0014,
					    b43legacy_phy_read(dev, 0x0014)
					    | 0x0200);
		}
		b43legacy_radio_write16(dev, 0x007A,
					b43legacy_radio_read16(dev, 0x007A)
					| 0x0070);
		b43legacy_set_all_gains(dev, 0, 8, 0);
		b43legacy_radio_write16(dev, 0x007A,
					b43legacy_radio_read16(dev, 0x007A)
					& 0x00F7);
		if (phy->rev >= 2) {
			b43legacy_phy_write(dev, 0x0811,
					    (b43legacy_phy_read(dev, 0x0811)
					    & 0xFFCF) | 0x0030);
			b43legacy_phy_write(dev, 0x0812,
					    (b43legacy_phy_read(dev, 0x0812)
					    & 0xFFCF) | 0x0010);
		}
		b43legacy_radio_write16(dev, 0x007A,
					b43legacy_radio_read16(dev, 0x007A)
					| 0x0080);
		udelay(20);

		nrssi0 = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
		if (nrssi0 >= 0x0020)
			nrssi0 -= 0x0040;

		b43legacy_radio_write16(dev, 0x007A,
					b43legacy_radio_read16(dev, 0x007A)
					& 0x007F);
		if (phy->analog >= 2)
			b43legacy_phy_write(dev, 0x0003,
					    (b43legacy_phy_read(dev, 0x0003)
					    & 0xFF9F) | 0x0040);

		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT,
				  b43legacy_read16(dev,
				  B43legacy_MMIO_CHANNEL_EXT) | 0x2000);
		b43legacy_radio_write16(dev, 0x007A,
					b43legacy_radio_read16(dev, 0x007A)
					| 0x000F);
		b43legacy_phy_write(dev, 0x0015, 0xF330);
		if (phy->rev >= 2) {
			b43legacy_phy_write(dev, 0x0812,
					    (b43legacy_phy_read(dev, 0x0812)
					    & 0xFFCF) | 0x0020);
			b43legacy_phy_write(dev, 0x0811,
					    (b43legacy_phy_read(dev, 0x0811)
					    & 0xFFCF) | 0x0020);
		}

		b43legacy_set_all_gains(dev, 3, 0, 1);
		if (phy->radio_rev == 8)
			b43legacy_radio_write16(dev, 0x0043, 0x001F);
		else {
			tmp = b43legacy_radio_read16(dev, 0x0052) & 0xFF0F;
			b43legacy_radio_write16(dev, 0x0052, tmp | 0x0060);
			tmp = b43legacy_radio_read16(dev, 0x0043) & 0xFFF0;
			b43legacy_radio_write16(dev, 0x0043, tmp | 0x0009);
		}
		b43legacy_phy_write(dev, 0x005A, 0x0480);
		b43legacy_phy_write(dev, 0x0059, 0x0810);
		b43legacy_phy_write(dev, 0x0058, 0x000D);
		udelay(20);
		nrssi1 = (s16)((b43legacy_phy_read(dev, 0x047F) >> 8) & 0x003F);
		if (nrssi1 >= 0x0020)
			nrssi1 -= 0x0040;
		if (nrssi0 == nrssi1)
			phy->nrssislope = 0x00010000;
		else
			phy->nrssislope = 0x00400000 / (nrssi0 - nrssi1);
		if (nrssi0 >= -4) {
			phy->nrssi[0] = nrssi1;
			phy->nrssi[1] = nrssi0;
		}
		if (phy->rev >= 3) {
			b43legacy_phy_write(dev, 0x002E, backup[10]);
			b43legacy_phy_write(dev, 0x002F, backup[11]);
			b43legacy_phy_write(dev, 0x080F, backup[12]);
			b43legacy_phy_write(dev, B43legacy_PHY_G_LO_CONTROL,
					    backup[13]);
		}
		if (phy->rev >= 2) {
			b43legacy_phy_write(dev, 0x0812,
					    b43legacy_phy_read(dev, 0x0812)
					    & 0xFFCF);
			b43legacy_phy_write(dev, 0x0811,
					    b43legacy_phy_read(dev, 0x0811)
					    & 0xFFCF);
		}

		b43legacy_radio_write16(dev, 0x007A, backup[0]);
		b43legacy_radio_write16(dev, 0x0052, backup[1]);
		b43legacy_radio_write16(dev, 0x0043, backup[2]);
		b43legacy_write16(dev, 0x03E2, backup[7]);
		b43legacy_write16(dev, 0x03E6, backup[8]);
		b43legacy_write16(dev, B43legacy_MMIO_CHANNEL_EXT, backup[9]);
		b43legacy_phy_write(dev, 0x0015, backup[3]);
		b43legacy_phy_write(dev, 0x005A, backup[4]);
		b43legacy_phy_write(dev, 0x0059, backup[5]);
		b43legacy_phy_write(dev, 0x0058, backup[6]);
		b43legacy_synth_pu_workaround(dev, phy->channel);
		b43legacy_phy_write(dev, 0x0802,
				    b43legacy_phy_read(dev, 0x0802) | 0x0003);
		b43legacy_set_original_gains(dev);
		b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
				    b43legacy_phy_read(dev, B43legacy_PHY_G_CRS)
				    | 0x8000);
		if (phy->rev >= 3) {
			b43legacy_phy_write(dev, 0x0801, backup[14]);
			b43legacy_phy_write(dev, 0x0060, backup[15]);
			b43legacy_phy_write(dev, 0x0014, backup[16]);
			b43legacy_phy_write(dev, 0x0478, backup[17]);
		}
		b43legacy_nrssi_mem_update(dev);
		b43legacy_calc_nrssi_threshold(dev);
		break;
	default:
		B43legacy_BUG_ON(1);
	}
}

void b43legacy_calc_nrssi_threshold(struct b43legacy_wldev *dev)
{
	struct b43legacy_phy *phy = &dev->phy;
	s32 threshold;
	s32 a;
	s32 b;
	s16 tmp16;
	u16 tmp_u16;

	switch (phy->type) {
	case B43legacy_PHYTYPE_B: {
		if (phy->radio_ver != 0x2050)
			return;
		if (!(dev->dev->bus->sprom.r1.boardflags_lo &
		    B43legacy_BFL_RSSI))
			return;

		if (phy->radio_rev >= 6) {
			threshold = (phy->nrssi[1] - phy->nrssi[0]) * 32;
			threshold += 20 * (phy->nrssi[0] + 1);
			threshold /= 40;
		} else
			threshold = phy->nrssi[1] - 5;

		threshold = limit_value(threshold, 0, 0x3E);
		b43legacy_phy_read(dev, 0x0020); /* dummy read */
		b43legacy_phy_write(dev, 0x0020, (((u16)threshold) << 8)
				    | 0x001C);

		if (phy->radio_rev >= 6) {
			b43legacy_phy_write(dev, 0x0087, 0x0E0D);
			b43legacy_phy_write(dev, 0x0086, 0x0C0B);
			b43legacy_phy_write(dev, 0x0085, 0x0A09);
			b43legacy_phy_write(dev, 0x0084, 0x0808);
			b43legacy_phy_write(dev, 0x0083, 0x0808);
			b43legacy_phy_write(dev, 0x0082, 0x0604);
			b43legacy_phy_write(dev, 0x0081, 0x0302);
			b43legacy_phy_write(dev, 0x0080, 0x0100);
		}
		break;
	}
	case B43legacy_PHYTYPE_G:
		if (!phy->gmode ||
		    !(dev->dev->bus->sprom.r1.boardflags_lo &
		    B43legacy_BFL_RSSI)) {
			tmp16 = b43legacy_nrssi_hw_read(dev, 0x20);
			if (tmp16 >= 0x20)
				tmp16 -= 0x40;
			if (tmp16 < 3)
				b43legacy_phy_write(dev, 0x048A,
						    (b43legacy_phy_read(dev,
						    0x048A) & 0xF000) | 0x09EB);
			else
				b43legacy_phy_write(dev, 0x048A,
						    (b43legacy_phy_read(dev,
						    0x048A) & 0xF000) | 0x0AED);
		} else {
			if (phy->interfmode ==
			    B43legacy_RADIO_INTERFMODE_NONWLAN) {
				a = 0xE;
				b = 0xA;
			} else if (!phy->aci_wlan_automatic &&
				    phy->aci_enable) {
				a = 0x13;
				b = 0x12;
			} else {
				a = 0xE;
				b = 0x11;
			}

			a = a * (phy->nrssi[1] - phy->nrssi[0]);
			a += (phy->nrssi[0] << 6);
			if (a < 32)
				a += 31;
			else
				a += 32;
			a = a >> 6;
			a = limit_value(a, -31, 31);

			b = b * (phy->nrssi[1] - phy->nrssi[0]);
			b += (phy->nrssi[0] << 6);
			if (b < 32)
				b += 31;
			else
				b += 32;
			b = b >> 6;
			b = limit_value(b, -31, 31);

			tmp_u16 = b43legacy_phy_read(dev, 0x048A) & 0xF000;
			tmp_u16 |= ((u32)b & 0x0000003F);
			tmp_u16 |= (((u32)a & 0x0000003F) << 6);
			b43legacy_phy_write(dev, 0x048A, tmp_u16);
		}
		break;
	default:
		B43legacy_BUG_ON(1);
	}
}

/* Stack implementation to save/restore values from the
 * interference mitigation code.
 * It is save to restore values in random order.
 */
static void _stack_save(u32 *_stackptr, size_t *stackidx,
			u8 id, u16 offset, u16 value)
{
	u32 *stackptr = &(_stackptr[*stackidx]);

	B43legacy_WARN_ON(!((offset & 0xE000) == 0x0000));
	B43legacy_WARN_ON(!((id & 0xF8) == 0x00));
	*stackptr = offset;
	*stackptr |= ((u32)id) << 13;
	*stackptr |= ((u32)value) << 16;
	(*stackidx)++;
	B43legacy_WARN_ON(!(*stackidx < B43legacy_INTERFSTACK_SIZE));
}

static u16 _stack_restore(u32 *stackptr,
			  u8 id, u16 offset)
{
	size_t i;

	B43legacy_WARN_ON(!((offset & 0xE000) == 0x0000));
	B43legacy_WARN_ON(!((id & 0xF8) == 0x00));
	for (i = 0; i < B43legacy_INTERFSTACK_SIZE; i++, stackptr++) {
		if ((*stackptr & 0x00001FFF) != offset)
			continue;
		if (((*stackptr & 0x00007000) >> 13) != id)
			continue;
		return ((*stackptr & 0xFFFF0000) >> 16);
	}
	B43legacy_BUG_ON(1);

	return 0;
}

#define phy_stacksave(offset)					\
	do {							\
		_stack_save(stack, &stackidx, 0x1, (offset),	\
			    b43legacy_phy_read(dev, (offset)));	\
	} while (0)
#define phy_stackrestore(offset)				\
	do {							\
		b43legacy_phy_write(dev, (offset),		\
				    _stack_restore(stack, 0x1,	\
				    (offset)));			\
	} while (0)
#define radio_stacksave(offset)						\
	do {								\
		_stack_save(stack, &stackidx, 0x2, (offset),		\
			    b43legacy_radio_read16(dev, (offset)));	\
	} while (0)
#define radio_stackrestore(offset)					\
	do {								\
		b43legacy_radio_write16(dev, (offset),			\
					_stack_restore(stack, 0x2,	\
					(offset)));			\
	} while (0)
#define ilt_stacksave(offset)					\
	do {							\
		_stack_save(stack, &stackidx, 0x3, (offset),	\
			    b43legacy_ilt_read(dev, (offset)));	\
	} while (0)
#define ilt_stackrestore(offset)				\
	do {							\
		b43legacy_ilt_write(dev, (offset),		\
				  _stack_restore(stack, 0x3,	\
						 (offset)));	\
	} while (0)

static void
b43legacy_radio_interference_mitigation_enable(struct b43legacy_wldev *dev,
					       int mode)
{
	struct b43legacy_phy *phy = &dev->phy;
	u16 tmp;
	u16 flipped;
	u32 tmp32;
	size_t stackidx = 0;
	u32 *stack = phy->interfstack;

	switch (mode) {
	case B43legacy_RADIO_INTERFMODE_NONWLAN:
		if (phy->rev != 1) {
			b43legacy_phy_write(dev, 0x042B,
					    b43legacy_phy_read(dev, 0x042B)
					    | 0x0800);
			b43legacy_phy_write(dev, B43legacy_PHY_G_CRS,
					    b43legacy_phy_read(dev,
					    B43legacy_PHY_G_CRS) & ~0x4000);
			break;
		}
		radio_stacksave(0x0078);
		tmp = (b43legacy_radio_read16(dev, 0x0078) & 0x001E);
		flipped = flip_4bit(tmp);
		if (flipped < 10 && flipped >= 8)
			flipped = 7;
		else if (flipped >= 10)
			flipped -= 3;
		flipped = flip_4bit(flipped);
		flipped = (flipped << 1) | 0x0020;
		b43legacy_radio_write16(dev, 0x0078, flipped);

		b43legacy_calc_nrssi_threshold(dev);

		phy_stacksave(0x0406);
		b43legacy_phy_write(dev, 0x0406, 0x7E28);

		b43legacy_phy_write(dev, 0x042B,
				    b43legacy_phy_read(dev, 0x042B) | 0x0800);
		b43legacy_phy_write(dev, B43legacy_PHY_RADIO_BITFIELD,
				    b43legacy_phy_read(dev,
				    B43legacy_PHY_RADIO_BITFIELD) | 0x1000);

		phy_stacksave(0x04A0);
		b43legacy_phy_write(dev, 0x04A0,
				    (b43legacy_phy_read(dev, 0x04A0) & 0xC0C0)
				    | 0x0008);
		phy_stacksave(0x04A1);
		b43legacy_phy_write(dev, 0x04A1,
				    (b43legacy_phy_read(dev, 0x04A1) & 0xC0C0)
				    | 0x0605);
		phy_stacksave(0x04A2);
		b43legacy_phy_write(dev, 0x04A2,
				    (b43legacy_phy_read(dev, 0x04A2) & 0xC0C0)
				    | 0x0204);
		phy_stacksave(0x04A8);
		b43legacy_phy_write(dev, 0x04A8,
				    (b43legacy_phy_read(dev, 0x04A8) & 0xC0C0)
				    | 0x0803);
		phy_stacksave(0x04AB);
		b43legacy_phy_write(dev, 0x04AB,
				    (b43legacy_phy_read(dev, 0x04AB) & 0xC0C0)
				    | 0x0605);

		phy_stacksave(0x04A7);
		b43legacy_phy_write(dev, 0x04A7, 0x0002);
		phy_stacksave(0x04A3);
		b43legacy_phy_write(dev, 0x04A3, 0x287A);
		phy_stacksave(0x04A9);
		b43legacy_phy_write(dev, 0x04A9, 0x2027);
		phy_stacksave(0x0493);
		b43legacy_phy_write(dev, 0x0493, 0x32F5);
		phy_stacksave(0x04AA);
		b43legacy_phy_write(dev, 0x04AA, 0x2027);
		phy_stacksave(0x04AC);
		b43legacy_phy_write(dev, 0x04AC, 0x32F5);
		break;
	case B43legacy_RADIO_INTERFMODE_MANUALWLAN:
		if (b43legacy_phy_read(dev, 0x0033) & 0x0800)
			break;

		phy->aci_enable = 1;

		phy_stacksave(B43legacy_PHY_RADIO_BITFIELD);
		phy_stacksave(B43legacy_PHY_G_CRS);
		if (phy->rev < 2)
			phy_stacksave(0x0406);
		else {
			phy_stacksave(0x04C0);
			phy_stacksave(0x04C1);
		}
		phy_stacksave(0x0033);
		phy_stacksave(0x04A7);