ASoC: Add FLL support for WM8400

Signed-off-by: Mark Brown <broonie@opensource.wolfsonmicro.com>

diff --git a/sound/soc/codecs/wm8400.c b/sound/soc/codecs/wm8400.c
index b7350c2..510efa6 100644 (file)
--- a/sound/soc/codecs/wm8400.c
+++ b/sound/soc/codecs/wm8400.c
@@ -70,6 +70,7 @@ struct wm8400_priv {
        unsigned int sysclk;
        unsigned int pcmclk;
        struct work_struct work;
+       int fll_in, fll_out;
 };
 
 static inline unsigned int wm8400_read(struct snd_soc_codec *codec,
@@ -931,6 +932,133 @@ static int wm8400_set_dai_sysclk(struct snd_soc_dai *codec_dai,
        return 0;
 }
 
+struct fll_factors {
+       u16 n;
+       u16 k;
+       u16 outdiv;
+       u16 fratio;
+       u16 freq_ref;
+};
+
+#define FIXED_FLL_SIZE ((1 << 16) * 10)
+
+static int fll_factors(struct wm8400_priv *wm8400, struct fll_factors *factors,
+                      unsigned int Fref, unsigned int Fout)
+{
+       u64 Kpart;
+       unsigned int K, Nmod, target;
+
+       factors->outdiv = 2;
+       while (Fout * factors->outdiv <  90000000 ||
+              Fout * factors->outdiv > 100000000) {
+               factors->outdiv *= 2;
+               if (factors->outdiv > 32) {
+                       dev_err(wm8400->wm8400->dev,
+                               "Unsupported FLL output frequency %dHz\n",
+                               Fout);
+                       return -EINVAL;
+               }
+       }
+       target = Fout * factors->outdiv;
+       factors->outdiv = factors->outdiv >> 2;
+
+       if (Fref < 48000)
+               factors->freq_ref = 1;
+       else
+               factors->freq_ref = 0;
+
+       if (Fref < 1000000)
+               factors->fratio = 9;
+       else
+               factors->fratio = 0;
+
+       /* Ensure we have a fractional part */
+       do {
+               if (Fref < 1000000)
+                       factors->fratio--;
+               else
+                       factors->fratio++;
+
+               if (factors->fratio < 1 || factors->fratio > 8) {
+                       dev_err(wm8400->wm8400->dev,
+                               "Unable to calculate FRATIO\n");
+                       return -EINVAL;
+               }
+
+               factors->n = target / (Fref * factors->fratio);
+               Nmod = target % (Fref * factors->fratio);
+       } while (Nmod == 0);
+
+       /* Calculate fractional part - scale up so we can round. */
+       Kpart = FIXED_FLL_SIZE * (long long)Nmod;
+
+       do_div(Kpart, (Fref * factors->fratio));
+
+       K = Kpart & 0xFFFFFFFF;
+
+       if ((K % 10) >= 5)
+               K += 5;
+
+       /* Move down to proper range now rounding is done */
+       factors->k = K / 10;
+
+       dev_dbg(wm8400->wm8400->dev,
+               "FLL: Fref=%d Fout=%d N=%x K=%x, FRATIO=%x OUTDIV=%x\n",
+               Fref, Fout,
+               factors->n, factors->k, factors->fratio, factors->outdiv);
+
+       return 0;
+}
+
+static int wm8400_set_dai_pll(struct snd_soc_dai *codec_dai, int pll_id,
+                             unsigned int freq_in, unsigned int freq_out)
+{
+       struct snd_soc_codec *codec = codec_dai->codec;
+       struct wm8400_priv *wm8400 = codec->private_data;
+       struct fll_factors factors;
+       int ret;
+       u16 reg;
+
+       if (freq_in == wm8400->fll_in && freq_out == wm8400->fll_out)
+               return 0;
+
+       if (freq_out != 0) {
+               ret = fll_factors(wm8400, &factors, freq_in, freq_out);
+               if (ret != 0)
+                       return ret;
+       }
+
+       wm8400->fll_out = freq_out;
+       wm8400->fll_in = freq_in;
+
+       /* We *must* disable the FLL before any changes */
+       reg = wm8400_read(codec, WM8400_POWER_MANAGEMENT_2);
+       reg &= ~WM8400_FLL_ENA;
+       wm8400_write(codec, WM8400_POWER_MANAGEMENT_2, reg);
+
+       reg = wm8400_read(codec, WM8400_FLL_CONTROL_1);
+       reg &= ~WM8400_FLL_OSC_ENA;
+       wm8400_write(codec, WM8400_FLL_CONTROL_1, reg);
+
+       if (freq_out == 0)
+               return 0;
+
+       reg &= ~(WM8400_FLL_REF_FREQ | WM8400_FLL_FRATIO_MASK);
+       reg |= WM8400_FLL_FRAC | factors.fratio;
+       reg |= factors.freq_ref << WM8400_FLL_REF_FREQ_SHIFT;
+       wm8400_write(codec, WM8400_FLL_CONTROL_1, reg);
+
+       wm8400_write(codec, WM8400_FLL_CONTROL_2, factors.k);
+       wm8400_write(codec, WM8400_FLL_CONTROL_3, factors.n);
+
+       reg = wm8400_read(codec, WM8400_FLL_CONTROL_4);
+       reg &= WM8400_FLL_OUTDIV_MASK;
+       reg |= factors.outdiv;
+       wm8400_write(codec, WM8400_FLL_CONTROL_4, reg);
+
+       return 0;
+}
+
 /*
  * Sets ADC and Voice DAC format.
  */
@@ -1188,6 +1316,7 @@ static struct snd_soc_dai_ops wm8400_dai_ops = {
        .set_fmt = wm8400_set_dai_fmt,
        .set_clkdiv = wm8400_set_dai_clkdiv,
        .set_sysclk = wm8400_set_dai_sysclk,
+       .set_pll = wm8400_set_dai_pll,
 };
 
 /*