rt2x00: Fix rt2800 txpower setting to correct value

TX_PWR_CFG_* setting need to consider below cases
-compesate 20M/40M tx power delta for 2.4/5GHZ band
-limit maximum EIRP tx power to power_level of
 regulatory requirement

Signed-off-by: RA-Jay Hung <jay_hung@ralinktech.com>
Signed-off-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

3 files changed, 199 insertions, 77 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2800.h b/drivers/net/wireless/rt2x00/rt2800.h
index ef8f605d1081..457887c85937 100644
--- a/drivers/net/wireless/rt2x00/rt2800.h
+++ b/drivers/net/wireless/rt2x00/rt2800.h
@@ -1703,11 +1703,14 @@ struct mac_iveiv_entry {
  */
 
 /*
- * BBP 1: TX Antenna & Power
- * POWER: 0 - normal, 1 - drop tx power by 6dBm, 2 - drop tx power by 12dBm,
- *	3 - increase tx power by 6dBm
- */
-#define BBP1_TX_POWER			FIELD8(0x07)
+ * BBP 1: TX Antenna & Power Control
+ * POWER_CTRL:
+ * 0 - normal,
+ * 1 - drop tx power by 6dBm,
+ * 2 - drop tx power by 12dBm,
+ * 3 - increase tx power by 6dBm
+ */
+#define BBP1_TX_POWER_CTRL		FIELD8(0x07)
 #define BBP1_TX_ANTENNA			FIELD8(0x18)
 
 /*
@@ -2001,23 +2004,26 @@ struct mac_iveiv_entry {
 #define EEPROM_RSSI_A2_LNA_A2		FIELD16(0xff00)
 
 /*
- * EEPROM Maximum TX power values
+ * EEPROM EIRP Maximum TX power values(unit: dbm)
  */
-#define EEPROM_MAX_TX_POWER		0x0027
-#define EEPROM_MAX_TX_POWER_24GHZ	FIELD16(0x00ff)
-#define EEPROM_MAX_TX_POWER_5GHZ	FIELD16(0xff00)
+#define EEPROM_EIRP_MAX_TX_POWER	0x0027
+#define EEPROM_EIRP_MAX_TX_POWER_2GHZ	FIELD16(0x00ff)
+#define EEPROM_EIRP_MAX_TX_POWER_5GHZ	FIELD16(0xff00)
 
 /*
  * EEPROM TXpower delta: 20MHZ AND 40 MHZ use different power.
  * This is delta in 40MHZ.
- * VALUE: Tx Power dalta value (MAX=4)
+ * VALUE: Tx Power dalta value, MAX=4(unit: dbm)
  * TYPE: 1: Plus the delta value, 0: minus the delta value
- * TXPOWER: Enable:
+ * ENABLE: enable tx power compensation for 40BW
  */
 #define EEPROM_TXPOWER_DELTA		0x0028
-#define EEPROM_TXPOWER_DELTA_VALUE	FIELD16(0x003f)
-#define EEPROM_TXPOWER_DELTA_TYPE	FIELD16(0x0040)
-#define EEPROM_TXPOWER_DELTA_TXPOWER	FIELD16(0x0080)
+#define EEPROM_TXPOWER_DELTA_VALUE_2G	FIELD16(0x003f)
+#define EEPROM_TXPOWER_DELTA_TYPE_2G	FIELD16(0x0040)
+#define EEPROM_TXPOWER_DELTA_ENABLE_2G	FIELD16(0x0080)
+#define EEPROM_TXPOWER_DELTA_VALUE_5G	FIELD16(0x3f00)
+#define EEPROM_TXPOWER_DELTA_TYPE_5G	FIELD16(0x4000)
+#define EEPROM_TXPOWER_DELTA_ENABLE_5G	FIELD16(0x8000)
 
 /*
  * EEPROM TXPOWER 802.11BG
@@ -2215,4 +2221,9 @@ struct mac_iveiv_entry {
 #define TXPOWER_A_TO_DEV(__txpower) \
 	clamp_t(char, __txpower, MIN_A_TXPOWER, MAX_A_TXPOWER)
 
+/*
+ *  Board's maximun TX power limitation
+ */
+#define EIRP_MAX_TX_POWER_LIMIT	0x50
+
 #endif /* RT2800_H */
diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index c62b4a593eb8..ab41f9e18ca9 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -1684,30 +1684,116 @@ static void rt2800_config_channel(struct rt2x00_dev *rt2x00dev,
 	rt2800_register_read(rt2x00dev, CH_BUSY_STA_SEC, &reg);
 }
 
+static int rt2800_get_txpower_bw_comp(struct rt2x00_dev *rt2x00dev,
+				      enum ieee80211_band band)
+{
+	u16 eeprom;
+	u8 comp_en;
+	u8 comp_type;
+	int comp_value;
+
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_TXPOWER_DELTA, &eeprom);
+
+	if (eeprom == 0xffff)
+		return 0;
+
+	if (band == IEEE80211_BAND_2GHZ) {
+		comp_en = rt2x00_get_field16(eeprom,
+				 EEPROM_TXPOWER_DELTA_ENABLE_2G);
+		if (comp_en) {
+			comp_type = rt2x00_get_field16(eeprom,
+					   EEPROM_TXPOWER_DELTA_TYPE_2G);
+			comp_value = rt2x00_get_field16(eeprom,
+					    EEPROM_TXPOWER_DELTA_VALUE_2G);
+			if (!comp_type)
+				comp_value = -comp_value;
+		}
+	} else {
+		comp_en = rt2x00_get_field16(eeprom,
+				 EEPROM_TXPOWER_DELTA_ENABLE_5G);
+		if (comp_en) {
+			comp_type = rt2x00_get_field16(eeprom,
+					   EEPROM_TXPOWER_DELTA_TYPE_5G);
+			comp_value = rt2x00_get_field16(eeprom,
+					    EEPROM_TXPOWER_DELTA_VALUE_5G);
+			if (!comp_type)
+				comp_value = -comp_value;
+		}
+	}
+
+	return comp_value;
+}
+
+static u8 rt2800_compesate_txpower(struct rt2x00_dev *rt2x00dev,
+				     int is_rate_b,
+				     enum ieee80211_band band,
+				     int power_level,
+				     u8 txpower)
+{
+	u32 reg;
+	u16 eeprom;
+	u8 criterion;
+	u8 eirp_txpower;
+	u8 eirp_txpower_criterion;
+	u8 reg_limit;
+	int bw_comp = 0;
+
+	if (!((band == IEEE80211_BAND_5GHZ) && is_rate_b))
+		return txpower;
+
+	if (test_bit(CONFIG_CHANNEL_HT40, &rt2x00dev->flags))
+		bw_comp = rt2800_get_txpower_bw_comp(rt2x00dev, band);
+
+	if (test_bit(CONFIG_SUPPORT_POWER_LIMIT, &rt2x00dev->flags)) {
+		/*
+		 * Check if eirp txpower exceed txpower_limit.
+		 * We use OFDM 6M as criterion and its eirp txpower
+		 * is stored at EEPROM_EIRP_MAX_TX_POWER.
+		 * .11b data rate need add additional 4dbm
+		 * when calculating eirp txpower.
+		 */
+		rt2800_register_read(rt2x00dev, TX_PWR_CFG_0, &reg);
+		criterion = rt2x00_get_field32(reg, TX_PWR_CFG_0_6MBS);
+
+		rt2x00_eeprom_read(rt2x00dev,
+				   EEPROM_EIRP_MAX_TX_POWER, &eeprom);
+
+		if (band == IEEE80211_BAND_2GHZ)
+			eirp_txpower_criterion = rt2x00_get_field16(eeprom,
+						 EEPROM_EIRP_MAX_TX_POWER_2GHZ);
+		else
+			eirp_txpower_criterion = rt2x00_get_field16(eeprom,
+						 EEPROM_EIRP_MAX_TX_POWER_5GHZ);
+
+		eirp_txpower = eirp_txpower_criterion + (txpower - criterion) +
+				       (is_rate_b ? 4 : 0) + bw_comp;
+
+		reg_limit = (eirp_txpower > power_level) ?
+					(eirp_txpower - power_level) : 0;
+	} else
+		reg_limit = 0;
+
+	return txpower + bw_comp - reg_limit;
+}
+
 static void rt2800_config_txpower(struct rt2x00_dev *rt2x00dev,
-				  const int max_txpower)
+				  struct ieee80211_conf *conf)
 {
 	u8 txpower;
-	u8 max_value = (u8)max_txpower;
 	u16 eeprom;
-	int i;
+	int i, is_rate_b;
 	u32 reg;
 	u8 r1;
 	u32 offset;
+	enum ieee80211_band band = conf->channel->band;
+	int power_level = conf->power_level;
 
 	/*
-	 * set to normal tx power mode: +/- 0dBm
+	 * set to normal bbp tx power control mode: +/- 0dBm
 	 */
 	rt2800_bbp_read(rt2x00dev, 1, &r1);
-	rt2x00_set_field8(&r1, BBP1_TX_POWER, 0);
+	rt2x00_set_field8(&r1, BBP1_TX_POWER_CTRL, 0);
 	rt2800_bbp_write(rt2x00dev, 1, r1);
-
-	/*
-	 * The eeprom contains the tx power values for each rate. These
-	 * values map to 100% tx power. Each 16bit word contains four tx
-	 * power values and the order is the same as used in the TX_PWR_CFG
-	 * registers.
-	 */
 	offset = TX_PWR_CFG_0;
 
 	for (i = 0; i < EEPROM_TXPOWER_BYRATE_SIZE; i += 2) {
@@ -1721,73 +1807,99 @@ static void rt2800_config_txpower(struct rt2x00_dev *rt2x00dev,
 		rt2x00_eeprom_read(rt2x00dev, EEPROM_TXPOWER_BYRATE + i,
 				   &eeprom);
 
-		/* TX_PWR_CFG_0: 1MBS, TX_PWR_CFG_1: 24MBS,
+		is_rate_b = i ? 0 : 1;
+		/*
+		 * TX_PWR_CFG_0: 1MBS, TX_PWR_CFG_1: 24MBS,
 		 * TX_PWR_CFG_2: MCS4, TX_PWR_CFG_3: MCS12,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE0);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE0,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE0, txpower);
 
-		/* TX_PWR_CFG_0: 2MBS, TX_PWR_CFG_1: 36MBS,
+		/*
+		 * TX_PWR_CFG_0: 2MBS, TX_PWR_CFG_1: 36MBS,
 		 * TX_PWR_CFG_2: MCS5, TX_PWR_CFG_3: MCS13,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE1);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE1,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE1, txpower);
 
-		/* TX_PWR_CFG_0: 55MBS, TX_PWR_CFG_1: 48MBS,
+		/*
+		 * TX_PWR_CFG_0: 5.5MBS, TX_PWR_CFG_1: 48MBS,
 		 * TX_PWR_CFG_2: MCS6,  TX_PWR_CFG_3: MCS14,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE2);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE2,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE2, txpower);
 
-		/* TX_PWR_CFG_0: 11MBS, TX_PWR_CFG_1: 54MBS,
+		/*
+		 * TX_PWR_CFG_0: 11MBS, TX_PWR_CFG_1: 54MBS,
 		 * TX_PWR_CFG_2: MCS7,  TX_PWR_CFG_3: MCS15,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE3);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE3,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE3, txpower);
 
 		/* read the next four txpower values */
 		rt2x00_eeprom_read(rt2x00dev, EEPROM_TXPOWER_BYRATE + i + 1,
 				   &eeprom);
 
-		/* TX_PWR_CFG_0: 6MBS, TX_PWR_CFG_1: MCS0,
+		is_rate_b = 0;
+		/*
+		 * TX_PWR_CFG_0: 6MBS, TX_PWR_CFG_1: MCS0,
 		 * TX_PWR_CFG_2: MCS8, TX_PWR_CFG_3: unknown,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE0);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE4,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE4, txpower);
 
-		/* TX_PWR_CFG_0: 9MBS, TX_PWR_CFG_1: MCS1,
+		/*
+		 * TX_PWR_CFG_0: 9MBS, TX_PWR_CFG_1: MCS1,
 		 * TX_PWR_CFG_2: MCS9, TX_PWR_CFG_3: unknown,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE1);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE5,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE5, txpower);
 
-		/* TX_PWR_CFG_0: 12MBS, TX_PWR_CFG_1: MCS2,
+		/*
+		 * TX_PWR_CFG_0: 12MBS, TX_PWR_CFG_1: MCS2,
 		 * TX_PWR_CFG_2: MCS10, TX_PWR_CFG_3: unknown,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE2);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE6,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE6, txpower);
 
-		/* TX_PWR_CFG_0: 18MBS, TX_PWR_CFG_1: MCS3,
+		/*
+		 * TX_PWR_CFG_0: 18MBS, TX_PWR_CFG_1: MCS3,
 		 * TX_PWR_CFG_2: MCS11, TX_PWR_CFG_3: unknown,
-		 * TX_PWR_CFG_4: unknown */
+		 * TX_PWR_CFG_4: unknown
+		 */
 		txpower = rt2x00_get_field16(eeprom,
 					     EEPROM_TXPOWER_BYRATE_RATE3);
-		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE7,
-				   min(txpower, max_value));
+		txpower = rt2800_compesate_txpower(rt2x00dev, is_rate_b, band,
+					     power_level, txpower);
+		rt2x00_set_field32(&reg, TX_PWR_CFG_RATE7, txpower);
 
 		rt2800_register_write(rt2x00dev, offset, reg);
 
@@ -1846,11 +1958,13 @@ void rt2800_config(struct rt2x00_dev *rt2x00dev,
 	/* Always recalculate LNA gain before changing configuration */
 	rt2800_config_lna_gain(rt2x00dev, libconf);
 
-	if (flags & IEEE80211_CONF_CHANGE_CHANNEL)
+	if (flags & IEEE80211_CONF_CHANGE_CHANNEL) {
 		rt2800_config_channel(rt2x00dev, libconf->conf,
 				      &libconf->rf, &libconf->channel);
+		rt2800_config_txpower(rt2x00dev, libconf->conf);
+	}
 	if (flags & IEEE80211_CONF_CHANGE_POWER)
-		rt2800_config_txpower(rt2x00dev, libconf->conf->power_level);
+		rt2800_config_txpower(rt2x00dev, libconf->conf);
 	if (flags & IEEE80211_CONF_CHANGE_RETRY_LIMITS)
 		rt2800_config_retry_limit(rt2x00dev, libconf);
 	if (flags & IEEE80211_CONF_CHANGE_PS)
@@ -3040,13 +3154,6 @@ int rt2800_validate_eeprom(struct rt2x00_dev *rt2x00dev)
 				   default_lna_gain);
 	rt2x00_eeprom_write(rt2x00dev, EEPROM_RSSI_A2, word);
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_MAX_TX_POWER, &word);
-	if (rt2x00_get_field16(word, EEPROM_MAX_TX_POWER_24GHZ) == 0xff)
-		rt2x00_set_field16(&word, EEPROM_MAX_TX_POWER_24GHZ, MAX_G_TXPOWER);
-	if (rt2x00_get_field16(word, EEPROM_MAX_TX_POWER_5GHZ) == 0xff)
-		rt2x00_set_field16(&word, EEPROM_MAX_TX_POWER_5GHZ, MAX_A_TXPOWER);
-	rt2x00_eeprom_write(rt2x00dev, EEPROM_MAX_TX_POWER, word);
-
 	return 0;
 }
 EXPORT_SYMBOL_GPL(rt2800_validate_eeprom);
@@ -3162,6 +3269,15 @@ int rt2800_init_eeprom(struct rt2x00_dev *rt2x00dev)
 	rt2x00_eeprom_read(rt2x00dev, EEPROM_FREQ, &rt2x00dev->led_mcu_reg);
 #endif /* CONFIG_RT2X00_LIB_LEDS */
 
+	/*
+	 * Check if support EIRP tx power limit feature.
+	 */
+	rt2x00_eeprom_read(rt2x00dev, EEPROM_EIRP_MAX_TX_POWER, &eeprom);
+
+	if (rt2x00_get_field16(eeprom, EEPROM_EIRP_MAX_TX_POWER_2GHZ) <
+					EIRP_MAX_TX_POWER_LIMIT)
+		__set_bit(CONFIG_SUPPORT_POWER_LIMIT, &rt2x00dev->flags);
+
 	return 0;
 }
 EXPORT_SYMBOL_GPL(rt2800_init_eeprom);
@@ -3314,7 +3430,6 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 	char *default_power1;
 	char *default_power2;
 	unsigned int i;
-	unsigned short max_power;
 	u16 eeprom;
 
 	/*
@@ -3439,26 +3554,21 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
 
 	spec->channels_info = info;
 
-	rt2x00_eeprom_read(rt2x00dev, EEPROM_MAX_TX_POWER, &eeprom);
-	max_power = rt2x00_get_field16(eeprom, EEPROM_MAX_TX_POWER_24GHZ);
 	default_power1 = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_BG1);
 	default_power2 = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_BG2);
 
 	for (i = 0; i < 14; i++) {
-		info[i].max_power = max_power;
-		info[i].default_power1 = TXPOWER_G_FROM_DEV(default_power1[i]);
-		info[i].default_power2 = TXPOWER_G_FROM_DEV(default_power2[i]);
+		info[i].default_power1 = default_power1[i];
+		info[i].default_power2 = default_power2[i];
 	}
 
 	if (spec->num_channels > 14) {
-		max_power = rt2x00_get_field16(eeprom, EEPROM_MAX_TX_POWER_5GHZ);
 		default_power1 = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_A1);
 		default_power2 = rt2x00_eeprom_addr(rt2x00dev, EEPROM_TXPOWER_A2);
 
 		for (i = 14; i < spec->num_channels; i++) {
-			info[i].max_power = max_power;
-			info[i].default_power1 = TXPOWER_A_FROM_DEV(default_power1[i]);
-			info[i].default_power2 = TXPOWER_A_FROM_DEV(default_power2[i]);
+			info[i].default_power1 = default_power1[i];
+			info[i].default_power2 = default_power2[i];
 		}
 	}
 
diff --git a/drivers/net/wireless/rt2x00/rt2x00.h b/drivers/net/wireless/rt2x00/rt2x00.h
index fd28836a0072..64c6ef52fe56 100644
--- a/drivers/net/wireless/rt2x00/rt2x00.h
+++ b/drivers/net/wireless/rt2x00/rt2x00.h
@@ -667,6 +667,7 @@ enum rt2x00_flags {
 	 */
 	CONFIG_SUPPORT_HW_BUTTON,
 	CONFIG_SUPPORT_HW_CRYPTO,
+	CONFIG_SUPPORT_POWER_LIMIT,
 	DRIVER_SUPPORT_CONTROL_FILTERS,
 	DRIVER_SUPPORT_CONTROL_FILTER_PSPOLL,
 	DRIVER_SUPPORT_PRE_TBTT_INTERRUPT,