2 files changed, 28 insertions, 26 deletions

diff --git a/sound/soc/sh/rcar/adg.c b/sound/soc/sh/rcar/adg.c
index 4a72fd74ddc2..197cb3ec075f 100644
--- a/sound/soc/sh/rcar/adg.c
+++ b/sound/soc/sh/rcar/adg.c
@@ -308,23 +308,12 @@ static void rsnd_adg_set_ssi_clk(struct rsnd_mod *ssi_mod, u32 val)
 	}
 }
 
-int rsnd_adg_ssi_clk_stop(struct rsnd_mod *ssi_mod)
-{
-	rsnd_adg_set_ssi_clk(ssi_mod, 0);
-
-	return 0;
-}
-
-int rsnd_adg_ssi_clk_try_start(struct rsnd_mod *ssi_mod, unsigned int rate)
+int rsnd_adg_clk_query(struct rsnd_priv *priv, unsigned int rate)
 {
-	struct rsnd_priv *priv = rsnd_mod_to_priv(ssi_mod);
 	struct rsnd_adg *adg = rsnd_priv_to_adg(priv);
 	struct device *dev = rsnd_priv_to_dev(priv);
-	struct rsnd_mod *adg_mod = rsnd_mod_get(adg);
 	struct clk *clk;
 	int i;
-	u32 data;
-	u32 ckr = 0;
 	int sel_table[] = {
 		[CLKA] = 0x1,
 		[CLKB] = 0x2,
@@ -338,30 +327,42 @@ int rsnd_adg_ssi_clk_try_start(struct rsnd_mod *ssi_mod, unsigned int rate)
 	 * find suitable clock from
 	 * AUDIO_CLKA/AUDIO_CLKB/AUDIO_CLKC/AUDIO_CLKI.
 	 */
-	data = 0;
 	for_each_rsnd_clk(clk, adg, i) {
-		if (rate == clk_get_rate(clk)) {
-			data = sel_table[i];
-			goto found_clock;
-		}
+		if (rate == clk_get_rate(clk))
+			return sel_table[i];
 	}
 
 	/*
 	 * find divided clock from BRGA/BRGB
 	 */
-	if (rate  == adg->rbga_rate_for_441khz) {
-		data = 0x10;
-		goto found_clock;
-	}
+	if (rate == adg->rbga_rate_for_441khz)
+		return 0x10;
 
-	if (rate == adg->rbgb_rate_for_48khz) {
-		data = 0x20;
-		goto found_clock;
-	}
+	if (rate == adg->rbgb_rate_for_48khz)
+		return 0x20;
 
 	return -EIO;
+}
+
+int rsnd_adg_ssi_clk_stop(struct rsnd_mod *ssi_mod)
+{
+	rsnd_adg_set_ssi_clk(ssi_mod, 0);
+
+	return 0;
+}
+
+int rsnd_adg_ssi_clk_try_start(struct rsnd_mod *ssi_mod, unsigned int rate)
+{
+	struct rsnd_priv *priv = rsnd_mod_to_priv(ssi_mod);
+	struct rsnd_adg *adg = rsnd_priv_to_adg(priv);
+	struct device *dev = rsnd_priv_to_dev(priv);
+	struct rsnd_mod *adg_mod = rsnd_mod_get(adg);
+	int data;
+	u32 ckr = 0;
 
-found_clock:
+	data = rsnd_adg_clk_query(priv, rate);
+	if (data < 0)
+		return data;
 
 	rsnd_adg_set_ssi_clk(ssi_mod, data);
 
diff --git a/sound/soc/sh/rcar/rsnd.h b/sound/soc/sh/rcar/rsnd.h
index 47162bdf1abc..7b76f3998fd7 100644
--- a/sound/soc/sh/rcar/rsnd.h
+++ b/sound/soc/sh/rcar/rsnd.h
@@ -502,6 +502,7 @@ phys_addr_t rsnd_gen_get_phy_addr(struct rsnd_priv *priv, int reg_id);
 /*
  *	R-Car ADG
  */
+int rsnd_adg_clk_query(struct rsnd_priv *priv, unsigned int rate);
 int rsnd_adg_ssi_clk_stop(struct rsnd_mod *mod);
 int rsnd_adg_ssi_clk_try_start(struct rsnd_mod *mod, unsigned int rate);
 int rsnd_adg_probe(struct rsnd_priv *priv);