1 files changed, 99 insertions, 0 deletions

diff --git a/drivers/clk/imx/clk-pllv3.c b/drivers/clk/imx/clk-pllv3.c
index ed3a2df536ea..f1099167ba31 100644
--- a/drivers/clk/imx/clk-pllv3.c
+++ b/drivers/clk/imx/clk-pllv3.c
@@ -21,6 +21,9 @@
 #define PLL_NUM_OFFSET		0x10
 #define PLL_DENOM_OFFSET	0x20
 
+#define PLL_VF610_NUM_OFFSET	0x20
+#define PLL_VF610_DENOM_OFFSET	0x30
+
 #define BM_PLL_POWER		(0x1 << 12)
 #define BM_PLL_LOCK		(0x1 << 31)
 #define IMX7_ENET_PLL_POWER	(0x1 << 5)
@@ -300,6 +303,99 @@ static const struct clk_ops clk_pllv3_av_ops = {
 	.set_rate	= clk_pllv3_av_set_rate,
 };
 
+struct clk_pllv3_vf610_mf {
+	u32 mfi;	/* integer part, can be 20 or 22 */
+	u32 mfn;	/* numerator, 30-bit value */
+	u32 mfd;	/* denominator, 30-bit value, must be less than mfn */
+};
+
+static unsigned long clk_pllv3_vf610_mf_to_rate(unsigned long parent_rate,
+		struct clk_pllv3_vf610_mf mf)
+{
+	u64 temp64;
+
+	temp64 = parent_rate;
+	temp64 *= mf.mfn;
+	do_div(temp64, mf.mfd);
+
+	return (parent_rate * mf.mfi) + temp64;
+}
+
+static struct clk_pllv3_vf610_mf clk_pllv3_vf610_rate_to_mf(
+		unsigned long parent_rate, unsigned long rate)
+{
+	struct clk_pllv3_vf610_mf mf;
+	u64 temp64;
+
+	mf.mfi = (rate >= 22 * parent_rate) ? 22 : 20;
+	mf.mfd = 0x3fffffff;	/* use max supported value for best accuracy */
+
+	if (rate <= parent_rate * mf.mfi)
+		mf.mfn = 0;
+	else if (rate >= parent_rate * (mf.mfi + 1))
+		mf.mfn = mf.mfd - 1;
+	else {
+		/* rate = parent_rate * (mfi + mfn/mfd) */
+		temp64 = rate - parent_rate * mf.mfi;
+		temp64 *= mf.mfd;
+		do_div(temp64, parent_rate);
+		mf.mfn = temp64;
+	}
+
+	return mf;
+}
+
+static unsigned long clk_pllv3_vf610_recalc_rate(struct clk_hw *hw,
+					      unsigned long parent_rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	struct clk_pllv3_vf610_mf mf;
+
+	mf.mfn = readl_relaxed(pll->base + PLL_VF610_NUM_OFFSET);
+	mf.mfd = readl_relaxed(pll->base + PLL_VF610_DENOM_OFFSET);
+	mf.mfi = (readl_relaxed(pll->base) & pll->div_mask) ? 22 : 20;
+
+	return clk_pllv3_vf610_mf_to_rate(parent_rate, mf);
+}
+
+static long clk_pllv3_vf610_round_rate(struct clk_hw *hw, unsigned long rate,
+				    unsigned long *prate)
+{
+	struct clk_pllv3_vf610_mf mf = clk_pllv3_vf610_rate_to_mf(*prate, rate);
+
+	return clk_pllv3_vf610_mf_to_rate(*prate, mf);
+}
+
+static int clk_pllv3_vf610_set_rate(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate)
+{
+	struct clk_pllv3 *pll = to_clk_pllv3(hw);
+	struct clk_pllv3_vf610_mf mf =
+			clk_pllv3_vf610_rate_to_mf(parent_rate, rate);
+	u32 val;
+
+	val = readl_relaxed(pll->base);
+	if (mf.mfi == 20)
+		val &= ~pll->div_mask;	/* clear bit for mfi=20 */
+	else
+		val |= pll->div_mask;	/* set bit for mfi=22 */
+	writel_relaxed(val, pll->base);
+
+	writel_relaxed(mf.mfn, pll->base + PLL_VF610_NUM_OFFSET);
+	writel_relaxed(mf.mfd, pll->base + PLL_VF610_DENOM_OFFSET);
+
+	return clk_pllv3_wait_lock(pll);
+}
+
+static const struct clk_ops clk_pllv3_vf610_ops = {
+	.prepare	= clk_pllv3_prepare,
+	.unprepare	= clk_pllv3_unprepare,
+	.is_prepared	= clk_pllv3_is_prepared,
+	.recalc_rate	= clk_pllv3_vf610_recalc_rate,
+	.round_rate	= clk_pllv3_vf610_round_rate,
+	.set_rate	= clk_pllv3_vf610_set_rate,
+};
+
 static unsigned long clk_pllv3_enet_recalc_rate(struct clk_hw *hw,
 						unsigned long parent_rate)
 {
@@ -334,6 +430,9 @@ struct clk *imx_clk_pllv3(enum imx_pllv3_type type, const char *name,
 	case IMX_PLLV3_SYS:
 		ops = &clk_pllv3_sys_ops;
 		break;
+	case IMX_PLLV3_SYS_VF610:
+		ops = &clk_pllv3_vf610_ops;
+		break;
 	case IMX_PLLV3_USB_VF610:
 		pll->div_shift = 1;
 	case IMX_PLLV3_USB: