ASoC: Add FLL support for WM8400 (e8523b64) · Commits · e / devices / android_kernel_fairphone_FP5

sound/soc/codecs/wm8400.c

+129 −0

Original line number	Diff line number	Diff line
		@@ -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,
		};

		/*