phy.c

linux内核源码

	s16 desired_pwr;
	s16 estimated_pwr;
	s16 pwr_adjust;
	s16 radio_att_delta;
	s16 baseband_att_delta;
	s16 radio_attenuation;
	s16 baseband_attenuation;
	unsigned long phylock_flags;

	if (phy->savedpctlreg == 0xFFFF)
		return;
	if ((dev->dev->bus->boardinfo.type == 0x0416) &&
	    is_bcm_board_vendor(dev))
		return;
#ifdef CONFIG_B43LEGACY_DEBUG
	if (phy->manual_txpower_control)
		return;
#endif

	B43legacy_BUG_ON(!(phy->type == B43legacy_PHYTYPE_B ||
			 phy->type == B43legacy_PHYTYPE_G));
	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x0058);
	v0 = (s8)(tmp & 0x00FF);
	v1 = (s8)((tmp & 0xFF00) >> 8);
	tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x005A);
	v2 = (s8)(tmp & 0x00FF);
	v3 = (s8)((tmp & 0xFF00) >> 8);
	tmp = 0;

	if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F || v3 == 0x7F) {
		tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
					 0x0070);
		v0 = (s8)(tmp & 0x00FF);
		v1 = (s8)((tmp & 0xFF00) >> 8);
		tmp = b43legacy_shm_read16(dev, B43legacy_SHM_SHARED,
					 0x0072);
		v2 = (s8)(tmp & 0x00FF);
		v3 = (s8)((tmp & 0xFF00) >> 8);
		if (v0 == 0x7F || v1 == 0x7F || v2 == 0x7F || v3 == 0x7F)
			return;
		v0 = (v0 + 0x20) & 0x3F;
		v1 = (v1 + 0x20) & 0x3F;
		v2 = (v2 + 0x20) & 0x3F;
		v3 = (v3 + 0x20) & 0x3F;
		tmp = 1;
	}
	b43legacy_radio_clear_tssi(dev);

	average = (v0 + v1 + v2 + v3 + 2) / 4;

	if (tmp && (b43legacy_shm_read16(dev, B43legacy_SHM_SHARED, 0x005E)
	    & 0x8))
		average -= 13;

	estimated_pwr = b43legacy_phy_estimate_power_out(dev, average);

	max_pwr = dev->dev->bus->sprom.r1.maxpwr_bg;

	if ((dev->dev->bus->sprom.r1.boardflags_lo
	     & B43legacy_BFL_PACTRL) &&
	    (phy->type == B43legacy_PHYTYPE_G))
		max_pwr -= 0x3;
	if (unlikely(max_pwr <= 0)) {
		b43legacywarn(dev->wl, "Invalid max-TX-power value in SPROM."
			"\n");
		max_pwr = 74; /* fake it */
		dev->dev->bus->sprom.r1.maxpwr_bg = max_pwr;
	}

	/* Use regulatory information to get the maximum power.
	 * In the absence of such data from mac80211, we will use 20 dBm, which
	 * is the value for the EU, US, Canada, and most of the world.
	 * The regulatory maximum is reduced by the antenna gain (from sprom)
	 * and 1.5 dBm (a safety factor??). The result is in Q5.2 format
	 * which accounts for the factor of 4 */
#define REG_MAX_PWR 20
	max_pwr = min(REG_MAX_PWR * 4 - dev->dev->bus->sprom.r1.antenna_gain_bg
		      - 0x6, max_pwr);

	/* find the desired power in Q5.2 - power_level is in dBm
	 * and limit it - max_pwr is already in Q5.2 */
	desired_pwr = limit_value(phy->power_level << 2, 0, max_pwr);
	if (b43legacy_debug(dev, B43legacy_DBG_XMITPOWER))
		b43legacydbg(dev->wl, "Current TX power output: " Q52_FMT
		       " dBm, Desired TX power output: " Q52_FMT
		       " dBm\n", Q52_ARG(estimated_pwr),
		       Q52_ARG(desired_pwr));
	/* Check if we need to adjust the current power. The factor of 2 is
	 * for damping */
	pwr_adjust = (desired_pwr - estimated_pwr) / 2;
	/* RF attenuation delta
	 * The minus sign is because lower attenuation => more power */
	radio_att_delta = -(pwr_adjust + 7) >> 3;
	/* Baseband attenuation delta */
	baseband_att_delta = -(pwr_adjust >> 1) - (4 * radio_att_delta);
	/* Do we need to adjust anything? */
	if ((radio_att_delta == 0) && (baseband_att_delta == 0)) {
		b43legacy_phy_lo_mark_current_used(dev);
		return;
	}

	/* Calculate the new attenuation values. */
	baseband_attenuation = phy->bbatt;
	baseband_attenuation += baseband_att_delta;
	radio_attenuation = phy->rfatt;
	radio_attenuation += radio_att_delta;

	/* Get baseband and radio attenuation values into permitted ranges.
	 * baseband 0-11, radio 0-9.
	 * Radio attenuation affects power level 4 times as much as baseband.
	 */
	if (radio_attenuation < 0) {
		baseband_attenuation -= (4 * -radio_attenuation);
		radio_attenuation = 0;
	} else if (radio_attenuation > 9) {
		baseband_attenuation += (4 * (radio_attenuation - 9));
		radio_attenuation = 9;
	} else {
		while (baseband_attenuation < 0 && radio_attenuation > 0) {
			baseband_attenuation += 4;
			radio_attenuation--;
		}
		while (baseband_attenuation > 11 && radio_attenuation < 9) {
			baseband_attenuation -= 4;
			radio_attenuation++;
		}
	}
	baseband_attenuation = limit_value(baseband_attenuation, 0, 11);

	txpower = phy->txctl1;
	if ((phy->radio_ver == 0x2050) && (phy->radio_rev == 2)) {
		if (radio_attenuation <= 1) {
			if (txpower == 0) {
				txpower = 3;
				radio_attenuation += 2;
				baseband_attenuation += 2;
			} else if (dev->dev->bus->sprom.r1.boardflags_lo
				   & B43legacy_BFL_PACTRL) {
				baseband_attenuation += 4 *
						     (radio_attenuation - 2);
				radio_attenuation = 2;
			}
		} else if (radio_attenuation > 4 && txpower != 0) {
			txpower = 0;
			if (baseband_attenuation < 3) {
				radio_attenuation -= 3;
				baseband_attenuation += 2;
			} else {
				radio_attenuation -= 2;
				baseband_attenuation -= 2;
			}
		}
	}
	/* Save the control values */
	phy->txctl1 = txpower;
	baseband_attenuation = limit_value(baseband_attenuation, 0, 11);
	radio_attenuation = limit_value(radio_attenuation, 0, 9);
	phy->rfatt = radio_attenuation;
	phy->bbatt = baseband_attenuation;

	/* Adjust the hardware */
	b43legacy_phy_lock(dev, phylock_flags);
	b43legacy_radio_lock(dev);
	b43legacy_radio_set_txpower_bg(dev, baseband_attenuation,
				       radio_attenuation, txpower);
	b43legacy_phy_lo_mark_current_used(dev);
	b43legacy_radio_unlock(dev);
	b43legacy_phy_unlock(dev, phylock_flags);
}

static inline
s32 b43legacy_tssi2dbm_ad(s32 num, s32 den)
{
	if (num < 0)
		return num/den;
	else
		return (num+den/2)/den;
}

static inline
s8 b43legacy_tssi2dbm_entry(s8 entry [], u8 index, s16 pab0, s16 pab1, s16 pab2)
{
	s32 m1;
	s32 m2;
	s32 f = 256;
	s32 q;
	s32 delta;
	s8 i = 0;

	m1 = b43legacy_tssi2dbm_ad(16 * pab0 + index * pab1, 32);
	m2 = max(b43legacy_tssi2dbm_ad(32768 + index * pab2, 256), 1);
	do {
		if (i > 15)
			return -EINVAL;
		q = b43legacy_tssi2dbm_ad(f * 4096 -
					  b43legacy_tssi2dbm_ad(m2 * f, 16) *
					  f, 2048);
		delta = abs(q - f);
		f = q;
		i++;
	} while (delta >= 2);
	entry[index] = limit_value(b43legacy_tssi2dbm_ad(m1 * f, 8192),
				   -127, 128);
	return 0;
}

/* http://bcm-specs.sipsolutions.net/TSSI_to_DBM_Table */
int b43legacy_phy_init_tssi2dbm_table(struct b43legacy_wldev *dev)
{
	struct b43legacy_phy *phy = &dev->phy;
	s16 pab0;
	s16 pab1;
	s16 pab2;
	u8 idx;
	s8 *dyn_tssi2dbm;

	B43legacy_WARN_ON(!(phy->type == B43legacy_PHYTYPE_B ||
			  phy->type == B43legacy_PHYTYPE_G));
	pab0 = (s16)(dev->dev->bus->sprom.r1.pa0b0);
	pab1 = (s16)(dev->dev->bus->sprom.r1.pa0b1);
	pab2 = (s16)(dev->dev->bus->sprom.r1.pa0b2);

	if ((dev->dev->bus->chip_id == 0x4301) && (phy->radio_ver != 0x2050)) {
		phy->idle_tssi = 0x34;
		phy->tssi2dbm = b43legacy_tssi2dbm_b_table;
		return 0;
	}

	if (pab0 != 0 && pab1 != 0 && pab2 != 0 &&
	    pab0 != -1 && pab1 != -1 && pab2 != -1) {
		/* The pabX values are set in SPROM. Use them. */
		if ((s8)dev->dev->bus->sprom.r1.itssi_bg != 0 &&
		    (s8)dev->dev->bus->sprom.r1.itssi_bg != -1)
			phy->idle_tssi = (s8)(dev->dev->bus->sprom.r1.itssi_bg);
		else
			phy->idle_tssi = 62;
		dyn_tssi2dbm = kmalloc(64, GFP_KERNEL);
		if (dyn_tssi2dbm == NULL) {
			b43legacyerr(dev->wl, "Could not allocate memory "
			       "for tssi2dbm table\n");
			return -ENOMEM;
		}
		for (idx = 0; idx < 64; idx++)
			if (b43legacy_tssi2dbm_entry(dyn_tssi2dbm, idx, pab0,
						     pab1, pab2)) {
				phy->tssi2dbm = NULL;
				b43legacyerr(dev->wl, "Could not generate "
				       "tssi2dBm table\n");
				kfree(dyn_tssi2dbm);
				return -ENODEV;
			}
		phy->tssi2dbm = dyn_tssi2dbm;
		phy->dyn_tssi_tbl = 1;
	} else {
		/* pabX values not set in SPROM. */
		switch (phy->type) {
		case B43legacy_PHYTYPE_B:
			phy->idle_tssi = 0x34;
			phy->tssi2dbm = b43legacy_tssi2dbm_b_table;
			break;
		case B43legacy_PHYTYPE_G:
			phy->idle_tssi = 0x34;
			phy->tssi2dbm = b43legacy_tssi2dbm_g_table;
			break;
		}
	}

	return 0;
}

int b43legacy_phy_init(struct b43legacy_wldev *dev)
{
	struct b43legacy_phy *phy = &dev->phy;
	int err = -ENODEV;

	switch (phy->type) {
	case B43legacy_PHYTYPE_B:
		switch (phy->rev) {
		case 2:
			b43legacy_phy_initb2(dev);
			err = 0;
			break;
		case 4:
			b43legacy_phy_initb4(dev);
			err = 0;
			break;
		case 5:
			b43legacy_phy_initb5(dev);
			err = 0;
			break;
		case 6:
			b43legacy_phy_initb6(dev);
			err = 0;
			break;
		}
		break;
	case B43legacy_PHYTYPE_G:
		b43legacy_phy_initg(dev);
		err = 0;
		break;
	}
	if (err)
		b43legacyerr(dev->wl, "Unknown PHYTYPE found\n");

	return err;
}

void b43legacy_phy_set_antenna_diversity(struct b43legacy_wldev *dev)
{
	struct b43legacy_phy *phy = &dev->phy;
	u16 antennadiv;
	u16 offset;
	u16 value;
	u32 ucodeflags;

	antennadiv = phy->antenna_diversity;

	if (antennadiv == 0xFFFF)
		antennadiv = 3;
	B43legacy_WARN_ON(antennadiv > 3);

	ucodeflags = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
					  B43legacy_UCODEFLAGS_OFFSET);
	b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
			      B43legacy_UCODEFLAGS_OFFSET,
			      ucodeflags & ~B43legacy_UCODEFLAG_AUTODIV);

	switch (phy->type) {
	case B43legacy_PHYTYPE_G:
		offset = 0x0400;

		if (antennadiv == 2)
			value = (3/*automatic*/ << 7);
		else
			value = (antennadiv << 7);
		b43legacy_phy_write(dev, offset + 1,
				    (b43legacy_phy_read(dev, offset + 1)
				    & 0x7E7F) | value);

		if (antennadiv >= 2) {
			if (antennadiv == 2)
				value = (antennadiv << 7);
			else
				value = (0/*force0*/ << 7);
			b43legacy_phy_write(dev, offset + 0x2B,
					    (b43legacy_phy_read(dev,
					    offset + 0x2B)
					    & 0xFEFF) | value);
		}

		if (phy->type == B43legacy_PHYTYPE_G) {
			if (antennadiv >= 2)
				b43legacy_phy_write(dev, 0x048C,
						    b43legacy_phy_read(dev,
						    0x048C) | 0x2000);
			else
				b43legacy_phy_write(dev, 0x048C,
						    b43legacy_phy_read(dev,
						    0x048C) & ~0x2000);
			if (phy->rev >= 2) {
				b43legacy_phy_write(dev, 0x0461,
						    b43legacy_phy_read(dev,
						    0x0461) | 0x0010);
				b43legacy_phy_write(dev, 0x04AD,
						    (b43legacy_phy_read(dev,
						    0x04AD)
						    & 0x00FF) | 0x0015);
				if (phy->rev == 2)
					b43legacy_phy_write(dev, 0x0427,
							    0x0008);
				else
					b43legacy_phy_write(dev, 0x0427,
						(b43legacy_phy_read(dev, 0x0427)
						 & 0x00FF) | 0x0008);
			} else if (phy->rev >= 6)
				b43legacy_phy_write(dev, 0x049B, 0x00DC);
		} else {
			if (phy->rev < 3)
				b43legacy_phy_write(dev, 0x002B,
						    (b43legacy_phy_read(dev,
						    0x002B) & 0x00FF)
						    | 0x0024);
			else {
				b43legacy_phy_write(dev, 0x0061,
						    b43legacy_phy_read(dev,
						    0x0061) | 0x0010);
				if (phy->rev == 3) {
					b43legacy_phy_write(dev, 0x0093,
							    0x001D);
					b43legacy_phy_write(dev, 0x0027,
							    0x0008);
				} else {
					b43legacy_phy_write(dev, 0x0093,
							    0x003A);
					b43legacy_phy_write(dev, 0x0027,
						(b43legacy_phy_read(dev, 0x0027)
						 & 0x00FF) | 0x0008);
				}
			}
		}
		break;
	case B43legacy_PHYTYPE_B:
		if (dev->dev->id.revision == 2)
			value = (3/*automatic*/ << 7);
		else
			value = (antennadiv << 7);
		b43legacy_phy_write(dev, 0x03E2,
				    (b43legacy_phy_read(dev, 0x03E2)
				    & 0xFE7F) | value);
		break;
	default:
		B43legacy_WARN_ON(1);
	}

	if (antennadiv >= 2) {
		ucodeflags = b43legacy_shm_read32(dev, B43legacy_SHM_SHARED,
						  B43legacy_UCODEFLAGS_OFFSET);
		b43legacy_shm_write32(dev, B43legacy_SHM_SHARED,
				      B43legacy_UCODEFLAGS_OFFSET,
				      ucodeflags | B43legacy_UCODEFLAG_AUTODIV);
	}

	phy->antenna_diversity = antennadiv;
}

/* Set the PowerSavingControlBits.
 * Bitvalues:
 *   0  => unset the bit
 *   1  => set the bit
 *   -1 => calculate the bit
 */
void b43legacy_power_saving_ctl_bits(struct b43legacy_wldev *dev,
				     int bit25, int bit26)
{
	int i;
	u32 status;

/* FIXME: Force 25 to off and 26 to on for now: */
bit25 = 0;
bit26 = 1;

	if (bit25 == -1) {
		/* TODO: If powersave is not off and FIXME is not set and we
		 *	are not in adhoc and thus is not an AP and we arei
		 *	a