2 files changed, 40 insertions, 22 deletions

diff --git a/arch/arm/include/asm/arch-tegra/clock.h b/arch/arm/include/asm/arch-tegra/clock.h
index a641a16a4b..04011ae255 100644
--- a/arch/arm/include/asm/arch-tegra/clock.h
+++ b/arch/arm/include/asm/arch-tegra/clock.h
@@ -156,6 +156,17 @@ void reset_cmplx_set_enable(int cpu, int which, int reset);
 void clock_ll_set_source(enum periph_id periph_id, unsigned source);
 
 /**
+ * This function is similar to clock_ll_set_source() except that it can be
+ * used for clocks with more than 2 mux bits.
+ *
+ * @param periph_id	peripheral to adjust
+ * @param mux_bits	number of mux bits for the clock
+ * @param source	source clock (0-15 depending on mux_bits)
+ */
+int clock_ll_set_source_bits(enum periph_id periph_id, int mux_bits,
+			     unsigned source);
+
+/**
  * Set the source and divisor for a peripheral clock. This sets the
  * clock rate. You need to look up the datasheet to see the meaning of the
  * source parameter as it changes for each peripheral.
diff --git a/arch/arm/mach-tegra/clock.c b/arch/arm/mach-tegra/clock.c
index e07f11d090..4b58cc187e 100644
--- a/arch/arm/mach-tegra/clock.c
+++ b/arch/arm/mach-tegra/clock.c
@@ -172,12 +172,37 @@ void clock_ll_set_source_divisor(enum periph_id periph_id, unsigned source,
 	writel(value, reg);
 }
 
-void clock_ll_set_source(enum periph_id periph_id, unsigned source)
+int clock_ll_set_source_bits(enum periph_id periph_id, int mux_bits,
+			     unsigned source)
 {
 	u32 *reg = get_periph_source_reg(periph_id);
 
-	clrsetbits_le32(reg, OUT_CLK_SOURCE_31_30_MASK,
-			source << OUT_CLK_SOURCE_31_30_SHIFT);
+	switch (mux_bits) {
+	case MASK_BITS_31_30:
+		clrsetbits_le32(reg, OUT_CLK_SOURCE_31_30_MASK,
+				source << OUT_CLK_SOURCE_31_30_SHIFT);
+		break;
+
+	case MASK_BITS_31_29:
+		clrsetbits_le32(reg, OUT_CLK_SOURCE_31_29_MASK,
+				source << OUT_CLK_SOURCE_31_29_SHIFT);
+		break;
+
+	case MASK_BITS_31_28:
+		clrsetbits_le32(reg, OUT_CLK_SOURCE_31_28_MASK,
+				source << OUT_CLK_SOURCE_31_28_SHIFT);
+		break;
+
+	default:
+		return -1;
+	}
+
+	return 0;
+}
+
+void clock_ll_set_source(enum periph_id periph_id, unsigned source)
+{
+	clock_ll_set_source_bits(periph_id, MASK_BITS_31_30, source);
 }
 
 /**
@@ -326,25 +351,7 @@ static int adjust_periph_pll(enum periph_id periph_id, int source,
 	if (source < 0)
 		return -1;
 
-	switch (mux_bits) {
-	case MASK_BITS_31_30:
-		clrsetbits_le32(reg, OUT_CLK_SOURCE_31_30_MASK,
-				source << OUT_CLK_SOURCE_31_30_SHIFT);
-		break;
-
-	case MASK_BITS_31_29:
-		clrsetbits_le32(reg, OUT_CLK_SOURCE_31_29_MASK,
-				source << OUT_CLK_SOURCE_31_29_SHIFT);
-		break;
-
-	case MASK_BITS_31_28:
-		clrsetbits_le32(reg, OUT_CLK_SOURCE_31_28_MASK,
-				source << OUT_CLK_SOURCE_31_28_SHIFT);
-		break;
-
-	default:
-		return -1;
-	}
+	clock_ll_set_source_bits(periph_id, mux_bits, source);
 
 	udelay(2);
 	return 0;