rt2800: add rt2800_normal_mode_setup_3xxx subroutine

Signed-off-by: Stanislaw Gruszka <stf_xl@wp.pl>
Acked-by: Gertjan van Wingerde <gwingerde@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

drivers/net/wireless/rt2x00/rt2800lib.c

@@ -4465,6 +4465,77 @@ static void rt2800_rx_filter_calibration(struct rt2x00_dev *rt2x00dev)
 	rt2800_bbp_write(rt2x00dev, 4, bbp);
 }
 
+static void rt2800_normal_mode_setup_3xxx(struct rt2x00_dev *rt2x00dev)
+{
+	struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
+	u8 min_gain, rfcsr, bbp;
+	u16 eeprom;
+
+	rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
+
+	rt2x00_set_field8(&rfcsr, RFCSR17_TX_LO1_EN, 0);
+	if (rt2x00_rt(rt2x00dev, RT3070) ||
+	    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
+	    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
+	    rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E)) {
+		if (!test_bit(CAPABILITY_EXTERNAL_LNA_BG, &rt2x00dev->cap_flags))
+			rt2x00_set_field8(&rfcsr, RFCSR17_R, 1);
+	}
+
+	min_gain = rt2x00_rt(rt2x00dev, RT3070) ? 1 : 2;
+	if (drv_data->txmixer_gain_24g >= min_gain) {
+		rt2x00_set_field8(&rfcsr, RFCSR17_TXMIXER_GAIN,
+				  drv_data->txmixer_gain_24g);
+	}
+
+	rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
+
+	if (rt2x00_rt(rt2x00dev, RT3090)) {
+		/*  Turn off unused DAC1 and ADC1 to reduce power consumption */
+		rt2800_bbp_read(rt2x00dev, 138, &bbp);
+		rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
+		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
+			rt2x00_set_field8(&bbp, BBP138_RX_ADC1, 0);
+		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) == 1)
+			rt2x00_set_field8(&bbp, BBP138_TX_DAC1, 1);
+		rt2800_bbp_write(rt2x00dev, 138, bbp);
+	}
+
+	if (rt2x00_rt(rt2x00dev, RT3070)) {
+		rt2800_rfcsr_read(rt2x00dev, 27, &rfcsr);
+		if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F))
+			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 3);
+		else
+			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR27_R2, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR27_R3, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR27_R4, 0);
+		rt2800_rfcsr_write(rt2x00dev, 27, rfcsr);
+	} else if (rt2x00_rt(rt2x00dev, RT3071) ||
+		   rt2x00_rt(rt2x00dev, RT3090) ||
+		   rt2x00_rt(rt2x00dev, RT3390)) {
+		rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
+		rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 0);
+		rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD, 1);
+		rt2x00_set_field8(&rfcsr, RFCSR1_TX1_PD, 1);
+		rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
+
+		rt2800_rfcsr_read(rt2x00dev, 15, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR15_TX_LO2_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 15, rfcsr);
+
+		rt2800_rfcsr_read(rt2x00dev, 20, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR20_RX_LO1_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 20, rfcsr);
+
+		rt2800_rfcsr_read(rt2x00dev, 21, &rfcsr);
+		rt2x00_set_field8(&rfcsr, RFCSR21_RX_LO2_EN, 0);
+		rt2800_rfcsr_write(rt2x00dev, 21, rfcsr);
+	}
+}
+
 static void rt2800_normal_mode_setup_5xxx(struct rt2x00_dev *rt2x00dev)
 {
 	u8 reg;
@@ -4599,6 +4670,7 @@ static void rt2800_init_rfcsr_30xx(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 27, 0x03);
 
 	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_3290(struct rt2x00_dev *rt2x00dev)
@@ -4659,6 +4731,7 @@ static void rt2800_init_rfcsr_3290(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 29, rfcsr);
 
 	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_3352(struct rt2x00_dev *rt2x00dev)
@@ -4730,8 +4803,8 @@ static void rt2800_init_rfcsr_3352(struct rt2x00_dev *rt2x00dev)
 	rt2800_rfcsr_write(rt2x00dev, 63, 0x00);
 
 	rt2800_rx_filter_calibration(rt2x00dev);
-
 	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_3390(struct rt2x00_dev *rt2x00dev)
@@ -4783,6 +4856,7 @@ static void rt2800_init_rfcsr_3390(struct rt2x00_dev *rt2x00dev)
 		rt2800_rfcsr_write(rt2x00dev, 27, 0x03);
 
 	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_3572(struct rt2x00_dev *rt2x00dev)
@@ -4839,8 +4913,8 @@ static void rt2800_init_rfcsr_3572(struct rt2x00_dev *rt2x00dev)
 	rt2800_register_write(rt2x00dev, LDO_CFG0, reg);
 
 	rt2800_rx_filter_calibration(rt2x00dev);
-
 	rt2800_led_open_drain_enable(rt2x00dev);
+	rt2800_normal_mode_setup_3xxx(rt2x00dev);
 }
 
 static void rt2800_init_rfcsr_5390(struct rt2x00_dev *rt2x00dev)
@@ -5048,12 +5122,6 @@ static void rt2800_init_rfcsr_5592(struct rt2x00_dev *rt2x00dev)
 
 static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 {
-	struct rt2800_drv_data *drv_data = rt2x00dev->drv_data;
-	u8 rfcsr;
-	u8 bbp;
-	u32 reg;
-	u16 eeprom;
-
 	if (!rt2x00_rt(rt2x00dev, RT3070) &&
 	    !rt2x00_rt(rt2x00dev, RT3071) &&
 	    !rt2x00_rt(rt2x00dev, RT3090) &&
@@ -5102,75 +5170,7 @@ static int rt2800_init_rfcsr(struct rt2x00_dev *rt2x00dev)
 		return 0;
 	}
 
-	if (!rt2x00_rt(rt2x00dev, RT5390) &&
-	    !rt2x00_rt(rt2x00dev, RT5392)) {
-		u8 min_gain = rt2x00_rt(rt2x00dev, RT3070) ? 1 : 2;
-
-		rt2800_rfcsr_read(rt2x00dev, 17, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR17_TX_LO1_EN, 0);
-		if (rt2x00_rt(rt2x00dev, RT3070) ||
-		    rt2x00_rt_rev_lt(rt2x00dev, RT3071, REV_RT3071E) ||
-		    rt2x00_rt_rev_lt(rt2x00dev, RT3090, REV_RT3090E) ||
-		    rt2x00_rt_rev_lt(rt2x00dev, RT3390, REV_RT3390E)) {
-			if (!test_bit(CAPABILITY_EXTERNAL_LNA_BG,
-				      &rt2x00dev->cap_flags))
-				rt2x00_set_field8(&rfcsr, RFCSR17_R, 1);
-		}
-		if (drv_data->txmixer_gain_24g >= min_gain) {
-			rt2x00_set_field8(&rfcsr, RFCSR17_TXMIXER_GAIN,
-					  drv_data->txmixer_gain_24g);
-		}
-		rt2800_rfcsr_write(rt2x00dev, 17, rfcsr);
-	}
-
-	if (rt2x00_rt(rt2x00dev, RT3090)) {
-		/*  Turn off unused DAC1 and ADC1 to reduce power consumption */
-		rt2800_bbp_read(rt2x00dev, 138, &bbp);
-		rt2x00_eeprom_read(rt2x00dev, EEPROM_NIC_CONF0, &eeprom);
-		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_RXPATH) == 1)
-			rt2x00_set_field8(&bbp, BBP138_RX_ADC1, 0);
-		if (rt2x00_get_field16(eeprom, EEPROM_NIC_CONF0_TXPATH) == 1)
-			rt2x00_set_field8(&bbp, BBP138_TX_DAC1, 1);
-		rt2800_bbp_write(rt2x00dev, 138, bbp);
-	}
-
-	if (rt2x00_rt(rt2x00dev, RT3071) ||
-	    rt2x00_rt(rt2x00dev, RT3090) ||
-	    rt2x00_rt(rt2x00dev, RT3390)) {
-		rt2800_rfcsr_read(rt2x00dev, 1, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR1_RF_BLOCK_EN, 1);
-		rt2x00_set_field8(&rfcsr, RFCSR1_RX0_PD, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR1_TX0_PD, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR1_RX1_PD, 1);
-		rt2x00_set_field8(&rfcsr, RFCSR1_TX1_PD, 1);
-		rt2800_rfcsr_write(rt2x00dev, 1, rfcsr);
-
-		rt2800_rfcsr_read(rt2x00dev, 15, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR15_TX_LO2_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 15, rfcsr);
-
-		rt2800_rfcsr_read(rt2x00dev, 20, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR20_RX_LO1_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 20, rfcsr);
-
-		rt2800_rfcsr_read(rt2x00dev, 21, &rfcsr);
-		rt2x00_set_field8(&rfcsr, RFCSR21_RX_LO2_EN, 0);
-		rt2800_rfcsr_write(rt2x00dev, 21, rfcsr);
-	}
-
-	if (rt2x00_rt(rt2x00dev, RT3070)) {
-		rt2800_rfcsr_read(rt2x00dev, 27, &rfcsr);
-		if (rt2x00_rt_rev_lt(rt2x00dev, RT3070, REV_RT3070F))
-			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 3);
-		else
-			rt2x00_set_field8(&rfcsr, RFCSR27_R1, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR27_R2, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR27_R3, 0);
-		rt2x00_set_field8(&rfcsr, RFCSR27_R4, 0);
-		rt2800_rfcsr_write(rt2x00dev, 27, rfcsr);
-	}
-
-	return 0;
+		return 0;
 }
 
 int rt2800_enable_radio(struct rt2x00_dev *rt2x00dev)