ASoC: Add FLL support for WM8400

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

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,
 };
 
 /*