1 files changed, 14 insertions, 93 deletions

diff --git a/sound/soc/codecs/rt5651.c b/sound/soc/codecs/rt5651.c
index 9c88d89..ea4b1c6 100644
--- a/sound/soc/codecs/rt5651.c
+++ b/sound/soc/codecs/rt5651.c
@@ -26,6 +26,7 @@
 #include <sound/initval.h>
 #include <sound/tlv.h>
 
+#include "rl6231.h"
 #include "rt5651.h"
 
 #define RT5651_DEVICE_ID_VALUE 0x6281
@@ -371,29 +372,16 @@ static const struct snd_kcontrol_new rt5651_snd_controls[] = {
  * @kcontrol: The kcontrol of this widget.
  * @event: Event id.
  *
- * Choose dmic clock between 1MHz and 3MHz.
- * It is better for clock to approximate 3MHz.
  */
 static int set_dmic_clk(struct snd_soc_dapm_widget *w,
 	struct snd_kcontrol *kcontrol, int event)
 {
 	struct snd_soc_codec *codec = w->codec;
 	struct rt5651_priv *rt5651 = snd_soc_codec_get_drvdata(codec);
-	int div[] = {2, 3, 4, 6, 8, 12}, idx = -EINVAL;
-	int i, rate, red, bound, temp;
-
-	rate = rt5651->sysclk;
-	red = 3000000 * 12;
-	for (i = 0; i < ARRAY_SIZE(div); i++) {
-		bound = div[i] * 3000000;
-		if (rate > bound)
-			continue;
-		temp = bound - rate;
-		if (temp < red) {
-			red = temp;
-			idx = i;
-		}
-	}
+	int idx = -EINVAL;
+
+	idx = rl6231_calc_dmic_clk(rt5651->sysclk);
+
 	if (idx < 0)
 		dev_err(codec->dev, "Failed to set DMIC clock\n");
 	else
@@ -1350,21 +1338,6 @@ static const struct snd_soc_dapm_route rt5651_dapm_routes[] = {
 	{"PDMR", NULL, "PDM R Mux"},
 };
 
-static int get_clk_info(int sclk, int rate)
-{
-	int i, pd[] = {1, 2, 3, 4, 6, 8, 12, 16};
-
-	if (sclk <= 0 || rate <= 0)
-		return -EINVAL;
-
-	rate = rate << 8;
-	for (i = 0; i < ARRAY_SIZE(pd); i++)
-		if (sclk == rate * pd[i])
-			return i;
-
-	return -EINVAL;
-}
-
 static int rt5651_hw_params(struct snd_pcm_substream *substream,
 	struct snd_pcm_hw_params *params, struct snd_soc_dai *dai)
 {
@@ -1374,7 +1347,7 @@ static int rt5651_hw_params(struct snd_pcm_substream *substream,
 	int pre_div, bclk_ms, frame_size;
 
 	rt5651->lrck[dai->id] = params_rate(params);
-	pre_div = get_clk_info(rt5651->sysclk, rt5651->lrck[dai->id]);
+	pre_div = rl6231_get_clk_info(rt5651->sysclk, rt5651->lrck[dai->id]);
 
 	if (pre_div < 0) {
 		dev_err(codec->dev, "Unsupported clock setting\n");
@@ -1528,65 +1501,12 @@ static int rt5651_set_dai_sysclk(struct snd_soc_dai *dai,
 	return 0;
 }
 
-/**
- * rt5651_pll_calc - Calcualte PLL M/N/K code.
- * @freq_in: external clock provided to codec.
- * @freq_out: target clock which codec works on.
- * @pll_code: Pointer to structure with M, N, K and bypass flag.
- *
- * Calcualte M/N/K code to configure PLL for codec. And K is assigned to 2
- * which make calculation more efficiently.
- *
- * Returns 0 for success or negative error code.
- */
-static int rt5651_pll_calc(const unsigned int freq_in,
-	const unsigned int freq_out, struct rt5651_pll_code *pll_code)
-{
-	int max_n = RT5651_PLL_N_MAX, max_m = RT5651_PLL_M_MAX;
-	int n = 0, m = 0, red, n_t, m_t, in_t, out_t;
-	int red_t = abs(freq_out - freq_in);
-	bool bypass = false;
-
-	if (RT5651_PLL_INP_MAX < freq_in || RT5651_PLL_INP_MIN > freq_in)
-		return -EINVAL;
-
-	for (n_t = 0; n_t <= max_n; n_t++) {
-		in_t = (freq_in >> 1) + (freq_in >> 2) * n_t;
-		if (in_t < 0)
-			continue;
-		if (in_t == freq_out) {
-			bypass = true;
-			n = n_t;
-			goto code_find;
-		}
-		for (m_t = 0; m_t <= max_m; m_t++) {
-			out_t = in_t / (m_t + 2);
-			red = abs(out_t - freq_out);
-			if (red < red_t) {
-				n = n_t;
-				m = m_t;
-				if (red == 0)
-					goto code_find;
-				red_t = red;
-			}
-		}
-	}
-	pr_debug("Only get approximation about PLL\n");
-
-code_find:
-	pll_code->m_bp = bypass;
-	pll_code->m_code = m;
-	pll_code->n_code = n;
-	pll_code->k_code = 2;
-	return 0;
-}
-
 static int rt5651_set_dai_pll(struct snd_soc_dai *dai, int pll_id, int source,
 			unsigned int freq_in, unsigned int freq_out)
 {
 	struct snd_soc_codec *codec = dai->codec;
 	struct rt5651_priv *rt5651 = snd_soc_codec_get_drvdata(codec);
-	struct rt5651_pll_code *pll_code = &rt5651->pll_code;
+	struct rl6231_pll_code pll_code;
 	int ret;
 
 	if (source == rt5651->pll_src && freq_in == rt5651->pll_in &&
@@ -1621,20 +1541,21 @@ static int rt5651_set_dai_pll(struct snd_soc_dai *dai, int pll_id, int source,
 		return -EINVAL;
 	}
 
-	ret = rt5651_pll_calc(freq_in, freq_out, pll_code);
+	ret = rl6231_pll_calc(freq_in, freq_out, &pll_code);
 	if (ret < 0) {
 		dev_err(codec->dev, "Unsupport input clock %d\n", freq_in);
 		return ret;
 	}
 
-	dev_dbg(codec->dev, "bypass=%d m=%d n=%d k=2\n", pll_code->m_bp,
-		(pll_code->m_bp ? 0 : pll_code->m_code), pll_code->n_code);
+	dev_dbg(codec->dev, "bypass=%d m=%d n=%d k=%d\n",
+		pll_code.m_bp, (pll_code.m_bp ? 0 : pll_code.m_code),
+		pll_code.n_code, pll_code.k_code);
 
 	snd_soc_write(codec, RT5651_PLL_CTRL1,
-		pll_code->n_code << RT5651_PLL_N_SFT | pll_code->k_code);
+		pll_code.n_code << RT5651_PLL_N_SFT | pll_code.k_code);
 	snd_soc_write(codec, RT5651_PLL_CTRL2,
-		(pll_code->m_bp ? 0 : pll_code->m_code) << RT5651_PLL_M_SFT |
-		pll_code->m_bp << RT5651_PLL_M_BP_SFT);
+		(pll_code.m_bp ? 0 : pll_code.m_code) << RT5651_PLL_M_SFT |
+		pll_code.m_bp << RT5651_PLL_M_BP_SFT);
 
 	rt5651->pll_in = freq_in;
 	rt5651->pll_out = freq_out;