1 files changed, 88 insertions, 0 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2800lib.c b/drivers/net/wireless/rt2x00/rt2800lib.c
index f1df929e97a..6104a14e4ff 100644
--- a/drivers/net/wireless/rt2x00/rt2800lib.c
+++ b/drivers/net/wireless/rt2x00/rt2800lib.c
@@ -2497,6 +2497,80 @@ void rt2800_gain_calibration(struct rt2x00_dev *rt2x00dev)
 }
 EXPORT_SYMBOL_GPL(rt2800_gain_calibration);
 
+void rt2800_vco_calibration(struct rt2x00_dev *rt2x00dev)
+{
+        u32     tx_pin;
+        u8      rfcsr;
+
+        /*
+         * A voltage-controlled oscillator(VCO) is an electronic oscillator
+         * designed to be controlled in oscillation frequency by a voltage
+         * input. Maybe the temperature will affect the frequency of
+         * oscillation to be shifted. The VCO calibration will be called
+         * periodically to adjust the frequency to be precision.
+        */
+
+        rt2800_register_read(rt2x00dev, TX_PIN_CFG, &tx_pin);
+        tx_pin &= TX_PIN_CFG_PA_PE_DISABLE;
+        rt2800_register_write(rt2x00dev, TX_PIN_CFG, tx_pin);
+
+        switch (rt2x00dev->chip.rf) {
+        case RF2020:
+        case RF3020:
+        case RF3021:
+        case RF3022:
+        case RF3320:
+        case RF3052:
+                rt2800_rfcsr_read(rt2x00dev, 7, &rfcsr);
+                rt2x00_set_field8(&rfcsr, RFCSR7_RF_TUNING, 1);
+                rt2800_rfcsr_write(rt2x00dev, 7, rfcsr);
+                break;
+        case RF5370:
+        case RF5372:
+        case RF5390:
+                rt2800_rfcsr_read(rt2x00dev, 3, &rfcsr);
+                rt2x00_set_field8(&rfcsr, RFCSR30_RF_CALIBRATION, 1);
+                rt2800_rfcsr_write(rt2x00dev, 3, rfcsr);
+                break;
+        default:
+                return;
+        }
+
+        mdelay(1);
+
+        rt2800_register_read(rt2x00dev, TX_PIN_CFG, &tx_pin);
+        if (rt2x00dev->rf_channel <= 14) {
+                switch (rt2x00dev->default_ant.tx_chain_num) {
+                case 3:
+                        rt2x00_set_field32(&tx_pin, TX_PIN_CFG_PA_PE_G2_EN, 1);
+                        /* fall through */
+                case 2:
+                        rt2x00_set_field32(&tx_pin, TX_PIN_CFG_PA_PE_G1_EN, 1);
+                        /* fall through */
+                case 1:
+                default:
+                        rt2x00_set_field32(&tx_pin, TX_PIN_CFG_PA_PE_G0_EN, 1);
+                        break;
+                }
+        } else {
+                switch (rt2x00dev->default_ant.tx_chain_num) {
+                case 3:
+                        rt2x00_set_field32(&tx_pin, TX_PIN_CFG_PA_PE_A2_EN, 1);
+                        /* fall through */
+                case 2:
+                        rt2x00_set_field32(&tx_pin, TX_PIN_CFG_PA_PE_A1_EN, 1);
+                        /* fall through */
+                case 1:
+                default:
+                        rt2x00_set_field32(&tx_pin, TX_PIN_CFG_PA_PE_A0_EN, 1);
+                        break;
+                }
+        }
+        rt2800_register_write(rt2x00dev, TX_PIN_CFG, tx_pin);
+
+}
+EXPORT_SYMBOL_GPL(rt2800_vco_calibration);
+
 static void rt2800_config_retry_limit(struct rt2x00_dev *rt2x00dev,
                                       struct rt2x00lib_conf *libconf)
 {
@@ -4451,6 +4525,20 @@ int rt2800_probe_hw_mode(struct rt2x00_dev *rt2x00dev)
                 }
         }
 
+        switch (rt2x00dev->chip.rf) {
+        case RF2020:
+        case RF3020:
+        case RF3021:
+        case RF3022:
+        case RF3320:
+        case RF3052:
+        case RF5370:
+        case RF5372:
+        case RF5390:
+                __set_bit(CAPABILITY_VCO_RECALIBRATION, &rt2x00dev->cap_flags);
+                break;
+        }
+
         return 0;
 }
 EXPORT_SYMBOL_GPL(rt2800_probe_hw_mode);