rt2x00:Add VCO recalibration

Signed-off-by: John Li <chen-yang.li@mediatek.com>
Acked-by: Gertjan van Wingerde <gwingerde@gmail.com>
Acked-by: Ivo van Doorn <IvDoorn@gmail.com>
Signed-off-by: John W. Linville <linville@tuxdriver.com>

1 files changed, 39 insertions, 0 deletions

diff --git a/drivers/net/wireless/rt2x00/rt2x00link.c b/drivers/net/wireless/rt2x00/rt2x00link.c
index ea10b0068f82..8368aab86f28 100644
--- a/drivers/net/wireless/rt2x00/rt2x00link.c
+++ b/drivers/net/wireless/rt2x00/rt2x00link.c
@@ -447,11 +447,27 @@ void rt2x00link_start_agc(struct rt2x00_dev *rt2x00dev)
                                              AGC_INTERVAL);
 }
 
+void rt2x00link_start_vcocal(struct rt2x00_dev *rt2x00dev)
+{
+        struct link *link = &rt2x00dev->link;
+
+        if (test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags) &&
+            rt2x00dev->ops->lib->vco_calibration)
+                ieee80211_queue_delayed_work(rt2x00dev->hw,
+                                             &link->vco_work,
+                                             VCO_INTERVAL);
+}
+
 void rt2x00link_stop_agc(struct rt2x00_dev *rt2x00dev)
 {
         cancel_delayed_work_sync(&rt2x00dev->link.agc_work);
 }
 
+void rt2x00link_stop_vcocal(struct rt2x00_dev *rt2x00dev)
+{
+        cancel_delayed_work_sync(&rt2x00dev->link.vco_work);
+}
+
 static void rt2x00link_agc(struct work_struct *work)
 {
         struct rt2x00_dev *rt2x00dev =
@@ -473,9 +489,32 @@ static void rt2x00link_agc(struct work_struct *work)
                                              AGC_INTERVAL);
 }
 
+static void rt2x00link_vcocal(struct work_struct *work)
+{
+        struct rt2x00_dev *rt2x00dev =
+            container_of(work, struct rt2x00_dev, link.vco_work.work);
+        struct link *link = &rt2x00dev->link;
+
+        /*
+         * When the radio is shutting down we should
+         * immediately cease the VCO calibration.
+         */
+        if (!test_bit(DEVICE_STATE_ENABLED_RADIO, &rt2x00dev->flags))
+                return;
+
+        rt2x00dev->ops->lib->vco_calibration(rt2x00dev);
+
+        if (test_bit(DEVICE_STATE_PRESENT, &rt2x00dev->flags))
+                ieee80211_queue_delayed_work(rt2x00dev->hw,
+                                             &link->vco_work,
+                                             VCO_INTERVAL);
+}
+
 void rt2x00link_register(struct rt2x00_dev *rt2x00dev)
 {
         INIT_DELAYED_WORK(&rt2x00dev->link.agc_work, rt2x00link_agc);
+        if (test_bit(CAPABILITY_VCO_RECALIBRATION, &rt2x00dev->cap_flags))
+                INIT_DELAYED_WORK(&rt2x00dev->link.vco_work, rt2x00link_vcocal);
         INIT_DELAYED_WORK(&rt2x00dev->link.watchdog_work, rt2x00link_watchdog);
         INIT_DELAYED_WORK(&rt2x00dev->link.work, rt2x00link_tuner);
 }