1 files changed, 168 insertions, 86 deletions

diff --git a/sound/soc/codecs/arizona.c b/sound/soc/codecs/arizona.c
index a32b84ac03f6..29e198f57d4c 100644
--- a/sound/soc/codecs/arizona.c
+++ b/sound/soc/codecs/arizona.c
@@ -53,6 +53,14 @@
 #define ARIZONA_AIF_RX_ENABLES                  0x1A
 #define ARIZONA_AIF_FORCE_WRITE                 0x1B
 
+#define ARIZONA_FLL_VCO_CORNER 141900000
+#define ARIZONA_FLL_MAX_FREF   13500000
+#define ARIZONA_FLL_MIN_FVCO   90000000
+#define ARIZONA_FLL_MAX_FRATIO 16
+#define ARIZONA_FLL_MAX_REFDIV 8
+#define ARIZONA_FLL_MIN_OUTDIV 2
+#define ARIZONA_FLL_MAX_OUTDIV 7
+
 #define arizona_fll_err(_fll, fmt, ...) \
         dev_err(_fll->arizona->dev, "FLL%d: " fmt, _fll->id, ##__VA_ARGS__)
 #define arizona_fll_warn(_fll, fmt, ...) \
@@ -1386,74 +1394,147 @@ struct arizona_fll_cfg {
         int gain;
 };
 
-static int arizona_calc_fll(struct arizona_fll *fll,
-                            struct arizona_fll_cfg *cfg,
-                            unsigned int Fref,
-                            unsigned int Fout)
+static int arizona_validate_fll(struct arizona_fll *fll,
+                                unsigned int Fref,
+                                unsigned int Fout)
 {
-        unsigned int target, div, gcd_fll;
-        int i, ratio;
+        unsigned int Fvco_min;
+
+        if (Fref / ARIZONA_FLL_MAX_REFDIV > ARIZONA_FLL_MAX_FREF) {
+                arizona_fll_err(fll,
+                                "Can't scale %dMHz in to <=13.5MHz\n",
+                                Fref);
+                return -EINVAL;
+        }
 
-        arizona_fll_dbg(fll, "Fref=%u Fout=%u\n", Fref, Fout);
+        Fvco_min = ARIZONA_FLL_MIN_FVCO * fll->vco_mult;
+        if (Fout * ARIZONA_FLL_MAX_OUTDIV < Fvco_min) {
+                arizona_fll_err(fll, "No FLL_OUTDIV for Fout=%uHz\n",
+                                Fout);
+                return -EINVAL;
+        }
+
+        return 0;
+}
+
+static int arizona_find_fratio(unsigned int Fref, int *fratio)
+{
+        int i;
+
+        /* Find an appropriate FLL_FRATIO */
+        for (i = 0; i < ARRAY_SIZE(fll_fratios); i++) {
+                if (fll_fratios[i].min <= Fref && Fref <= fll_fratios[i].max) {
+                        if (fratio)
+                                *fratio = fll_fratios[i].fratio;
+                        return fll_fratios[i].ratio;
+                }
+        }
+
+        return -EINVAL;
+}
+
+static int arizona_calc_fratio(struct arizona_fll *fll,
+                               struct arizona_fll_cfg *cfg,
+                               unsigned int target,
+                               unsigned int Fref, bool sync)
+{
+        int init_ratio, ratio;
+        int refdiv, div;
 
-        /* Fref must be <=13.5MHz */
+        /* Fref must be <=13.5MHz, find initial refdiv */
         div = 1;
         cfg->refdiv = 0;
-        while ((Fref / div) > 13500000) {
+        while (Fref > ARIZONA_FLL_MAX_FREF) {
                 div *= 2;
+                Fref /= 2;
                 cfg->refdiv++;
 
-                if (div > 8) {
-                        arizona_fll_err(fll,
-                                        "Can't scale %dMHz in to <=13.5MHz\n",
-                                        Fref);
+                if (div > ARIZONA_FLL_MAX_REFDIV)
                         return -EINVAL;
+        }
+
+        /* Find an appropriate FLL_FRATIO */
+        init_ratio = arizona_find_fratio(Fref, &cfg->fratio);
+        if (init_ratio < 0) {
+                arizona_fll_err(fll, "Unable to find FRATIO for Fref=%uHz\n",
+                                Fref);
+                return init_ratio;
+        }
+
+        switch (fll->arizona->type) {
+        case WM5110:
+                if (fll->arizona->rev < 3 || sync)
+                        return init_ratio;
+                break;
+        default:
+                return init_ratio;
+        }
+
+        cfg->fratio = init_ratio - 1;
+
+        /* Adjust FRATIO/refdiv to avoid integer mode if possible */
+        refdiv = cfg->refdiv;
+
+        while (div <= ARIZONA_FLL_MAX_REFDIV) {
+                for (ratio = init_ratio; ratio <= ARIZONA_FLL_MAX_FRATIO;
+                     ratio++) {
+                        if (target % (ratio * Fref)) {
+                                cfg->refdiv = refdiv;
+                                cfg->fratio = ratio - 1;
+                                return ratio;
+                        }
                 }
+
+                for (ratio = init_ratio - 1; ratio >= 0; ratio--) {
+                        if (ARIZONA_FLL_VCO_CORNER / (fll->vco_mult * ratio) <
+                            Fref)
+                                break;
+
+                        if (target % (ratio * Fref)) {
+                                cfg->refdiv = refdiv;
+                                cfg->fratio = ratio - 1;
+                                return ratio;
+                        }
+                }
+
+                div *= 2;
+                Fref /= 2;
+                refdiv++;
+                init_ratio = arizona_find_fratio(Fref, NULL);
         }
 
-        /* Apply the division for our remaining calculations */
-        Fref /= div;
+        arizona_fll_warn(fll, "Falling back to integer mode operation\n");
+        return cfg->fratio + 1;
+}
+
+static int arizona_calc_fll(struct arizona_fll *fll,
+                            struct arizona_fll_cfg *cfg,
+                            unsigned int Fref, bool sync)
+{
+        unsigned int target, div, gcd_fll;
+        int i, ratio;
+
+        arizona_fll_dbg(fll, "Fref=%u Fout=%u\n", Fref, fll->fout);
 
         /* Fvco should be over the targt; don't check the upper bound */
-        div = 1;
-        while (Fout * div < 90000000 * fll->vco_mult) {
+        div = ARIZONA_FLL_MIN_OUTDIV;
+        while (fll->fout * div < ARIZONA_FLL_MIN_FVCO * fll->vco_mult) {
                 div++;
-                if (div > 7) {
-                        arizona_fll_err(fll, "No FLL_OUTDIV for Fout=%uHz\n",
-                                        Fout);
+                if (div > ARIZONA_FLL_MAX_OUTDIV)
                         return -EINVAL;
-                }
         }
-        target = Fout * div / fll->vco_mult;
+        target = fll->fout * div / fll->vco_mult;
         cfg->outdiv = div;
 
         arizona_fll_dbg(fll, "Fvco=%dHz\n", target);
 
-        /* Find an appropraite FLL_FRATIO and factor it out of the target */
-        for (i = 0; i < ARRAY_SIZE(fll_fratios); i++) {
-                if (fll_fratios[i].min <= Fref && Fref <= fll_fratios[i].max) {
-                        cfg->fratio = fll_fratios[i].fratio;
-                        ratio = fll_fratios[i].ratio;
-                        break;
-                }
-        }
-        if (i == ARRAY_SIZE(fll_fratios)) {
-                arizona_fll_err(fll, "Unable to find FRATIO for Fref=%uHz\n",
-                                Fref);
-                return -EINVAL;
-        }
+        /* Find an appropriate FLL_FRATIO and refdiv */
+        ratio = arizona_calc_fratio(fll, cfg, target, Fref, sync);
+        if (ratio < 0)
+                return ratio;
 
-        for (i = 0; i < ARRAY_SIZE(fll_gains); i++) {
-                if (fll_gains[i].min <= Fref && Fref <= fll_gains[i].max) {
-                        cfg->gain = fll_gains[i].gain;
-                        break;
-                }
-        }
-        if (i == ARRAY_SIZE(fll_gains)) {
-                arizona_fll_err(fll, "Unable to find gain for Fref=%uHz\n",
-                                Fref);
-                return -EINVAL;
-        }
+        /* Apply the division for our remaining calculations */
+        Fref = Fref / (1 << cfg->refdiv);
 
         cfg->n = target / (ratio * Fref);
 
@@ -1478,6 +1559,18 @@ static int arizona_calc_fll(struct arizona_fll *fll,
                 cfg->lambda >>= 1;
         }
 
+        for (i = 0; i < ARRAY_SIZE(fll_gains); i++) {
+                if (fll_gains[i].min <= Fref && Fref <= fll_gains[i].max) {
+                        cfg->gain = fll_gains[i].gain;
+                        break;
+                }
+        }
+        if (i == ARRAY_SIZE(fll_gains)) {
+                arizona_fll_err(fll, "Unable to find gain for Fref=%uHz\n",
+                                Fref);
+                return -EINVAL;
+        }
+
         arizona_fll_dbg(fll, "N=%x THETA=%x LAMBDA=%x\n",
                         cfg->n, cfg->theta, cfg->lambda);
         arizona_fll_dbg(fll, "FRATIO=%x(%d) OUTDIV=%x REFCLK_DIV=%x\n",
@@ -1505,14 +1598,18 @@ static void arizona_apply_fll(struct arizona *arizona, unsigned int base,
                                  cfg->refdiv << ARIZONA_FLL1_CLK_REF_DIV_SHIFT |
                                  source << ARIZONA_FLL1_CLK_REF_SRC_SHIFT);
 
-        if (sync)
-                regmap_update_bits_async(arizona->regmap, base + 0x7,
-                                         ARIZONA_FLL1_GAIN_MASK,
-                                         cfg->gain << ARIZONA_FLL1_GAIN_SHIFT);
-        else
-                regmap_update_bits_async(arizona->regmap, base + 0x9,
-                                         ARIZONA_FLL1_GAIN_MASK,
-                                         cfg->gain << ARIZONA_FLL1_GAIN_SHIFT);
+        if (sync) {
+                regmap_update_bits(arizona->regmap, base + 0x7,
+                                   ARIZONA_FLL1_GAIN_MASK,
+                                   cfg->gain << ARIZONA_FLL1_GAIN_SHIFT);
+        } else {
+                regmap_update_bits(arizona->regmap, base + 0x5,
+                                   ARIZONA_FLL1_OUTDIV_MASK,
+                                   cfg->outdiv << ARIZONA_FLL1_OUTDIV_SHIFT);
+                regmap_update_bits(arizona->regmap, base + 0x9,
+                                   ARIZONA_FLL1_GAIN_MASK,
+                                   cfg->gain << ARIZONA_FLL1_GAIN_SHIFT);
+        }
 
         regmap_update_bits_async(arizona->regmap, base + 2,
                                  ARIZONA_FLL1_CTRL_UPD | ARIZONA_FLL1_N_MASK,
@@ -1535,13 +1632,12 @@ static bool arizona_is_enabled_fll(struct arizona_fll *fll)
         return reg & ARIZONA_FLL1_ENA;
 }
 
-static void arizona_enable_fll(struct arizona_fll *fll,
-                              struct arizona_fll_cfg *ref,
-                              struct arizona_fll_cfg *sync)
+static void arizona_enable_fll(struct arizona_fll *fll)
 {
         struct arizona *arizona = fll->arizona;
         int ret;
         bool use_sync = false;
+        struct arizona_fll_cfg cfg;
 
         /*
          * If we have both REFCLK and SYNCCLK then enable both,
@@ -1549,23 +1645,21 @@ static void arizona_enable_fll(struct arizona_fll *fll,
          */
         if (fll->ref_src >= 0 && fll->ref_freq &&
             fll->ref_src != fll->sync_src) {
-                regmap_update_bits_async(arizona->regmap, fll->base + 5,
-                                         ARIZONA_FLL1_OUTDIV_MASK,
-                                         ref->outdiv << ARIZONA_FLL1_OUTDIV_SHIFT);
+                arizona_calc_fll(fll, &cfg, fll->ref_freq, false);
 
-                arizona_apply_fll(arizona, fll->base, ref, fll->ref_src,
+                arizona_apply_fll(arizona, fll->base, &cfg, fll->ref_src,
                                   false);
                 if (fll->sync_src >= 0) {
-                        arizona_apply_fll(arizona, fll->base + 0x10, sync,
+                        arizona_calc_fll(fll, &cfg, fll->sync_freq, true);
+
+                        arizona_apply_fll(arizona, fll->base + 0x10, &cfg,
                                           fll->sync_src, true);
                         use_sync = true;
                 }
         } else if (fll->sync_src >= 0) {
-                regmap_update_bits_async(arizona->regmap, fll->base + 5,
-                                         ARIZONA_FLL1_OUTDIV_MASK,
-                                         sync->outdiv << ARIZONA_FLL1_OUTDIV_SHIFT);
+                arizona_calc_fll(fll, &cfg, fll->sync_freq, false);
 
-                arizona_apply_fll(arizona, fll->base, sync,
+                arizona_apply_fll(arizona, fll->base, &cfg,
                                   fll->sync_src, false);
 
                 regmap_update_bits_async(arizona->regmap, fll->base + 0x11,
@@ -1627,32 +1721,22 @@ static void arizona_disable_fll(struct arizona_fll *fll)
 int arizona_set_fll_refclk(struct arizona_fll *fll, int source,
                            unsigned int Fref, unsigned int Fout)
 {
-        struct arizona_fll_cfg ref, sync;
         int ret;
 
         if (fll->ref_src == source && fll->ref_freq == Fref)
                 return 0;
 
-        if (fll->fout) {
-                if (Fref > 0) {
-                        ret = arizona_calc_fll(fll, &ref, Fref, fll->fout);
-                        if (ret != 0)
-                                return ret;
-                }
-
-                if (fll->sync_src >= 0) {
-                        ret = arizona_calc_fll(fll, &sync, fll->sync_freq,
-                                               fll->fout);
-                        if (ret != 0)
-                                return ret;
-                }
+        if (fll->fout && Fref > 0) {
+                ret = arizona_validate_fll(fll, Fref, fll->fout);
+                if (ret != 0)
+                        return ret;
         }
 
         fll->ref_src = source;
         fll->ref_freq = Fref;
 
         if (fll->fout && Fref > 0) {
-                arizona_enable_fll(fll, &ref, &sync);
+                arizona_enable_fll(fll);
         }
 
         return 0;
@@ -1662,7 +1746,6 @@ EXPORT_SYMBOL_GPL(arizona_set_fll_refclk);
 int arizona_set_fll(struct arizona_fll *fll, int source,
                     unsigned int Fref, unsigned int Fout)
 {
-        struct arizona_fll_cfg ref, sync;
         int ret;
 
         if (fll->sync_src == source &&
@@ -1671,13 +1754,12 @@ int arizona_set_fll(struct arizona_fll *fll, int source,
 
         if (Fout) {
                 if (fll->ref_src >= 0) {
-                        ret = arizona_calc_fll(fll, &ref, fll->ref_freq,
-                                               Fout);
+                        ret = arizona_validate_fll(fll, fll->ref_freq, Fout);
                         if (ret != 0)
                                 return ret;
                 }
 
-                ret = arizona_calc_fll(fll, &sync, Fref, Fout);
+                ret = arizona_validate_fll(fll, Fref, Fout);
                 if (ret != 0)
                         return ret;
         }
@@ -1687,7 +1769,7 @@ int arizona_set_fll(struct arizona_fll *fll, int source,
         fll->fout = Fout;
 
         if (Fout) {
-                arizona_enable_fll(fll, &ref, &sync);
+                arizona_enable_fll(fll);
         } else {
                 arizona_disable_fll(fll);
         }