From 0ec1fea28c5569ad3bc59792ee07669aef222293 Mon Sep 17 00:00:00 2001
From: Qiufang Dai <qiufang.dai@amlogic.com>
Date: Thu, 13 Jul 2017 14:20:54 +0800
Subject: [PATCH] clk: axg: add pll enable op [1/1]

PD#146411: add enable op for pcie_gp0/hifi/mpll pll

Change-Id: I15eb279a0bf00035d2053322179dddc5d8d9d213
Signed-off-by: Qiufang Dai <qiufang.dai@amlogic.com>
---
 drivers/amlogic/clk/axg/axg_clk-pll.c | 58 +++++++++++++++++++++++++++++++++++
 drivers/amlogic/clk/clk-mpll.c        | 22 +++++++++++++
 2 files changed, 80 insertions(+)

diff --git a/drivers/amlogic/clk/axg/axg_clk-pll.c b/drivers/amlogic/clk/axg/axg_clk-pll.c
index bf5f393..e02b448 100644
--- a/drivers/amlogic/clk/axg/axg_clk-pll.c
+++ b/drivers/amlogic/clk/axg/axg_clk-pll.c
@@ -39,6 +39,7 @@
 #include <linux/slab.h>
 #include <linux/string.h>
 #include <linux/amlogic/cpu_version.h>
+#include <linux/clk.h>
 
 #ifdef CONFIG_ARM64
 #include "../clkc.h"
@@ -273,6 +274,62 @@ static int meson_axg_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 	return ret;
 }
 
+static int meson_axg_pll_enable(struct clk_hw *hw)
+{
+	struct meson_clk_pll *pll = to_meson_clk_pll(hw);
+	struct parm *p;
+	int ret = 0;
+	unsigned long flags = 0;
+	unsigned long first_set = 1;
+	struct clk *parent;
+	unsigned long rate;
+
+	p = &pll->n;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	if (readl(pll->base + p->reg_off) & MESON_PLL_ENABLE) {
+		if (pll->lock)
+			spin_unlock_irqrestore(pll->lock, flags);
+		return ret;
+	}
+
+	if (!strcmp(clk_hw_get_name(hw), "gp0_pll")
+		|| !strcmp(clk_hw_get_name(hw), "hifi_pll")
+		|| !strcmp(clk_hw_get_name(hw), "pcie_pll")) {
+		void *cntlbase = pll->base + p->reg_off;
+
+		if (!strcmp(clk_hw_get_name(hw), "pcie_pll")) {
+			if (readl(cntlbase + (u64)(6*4)) == AXG_PCIE_PLL_CNTL6)
+				first_set = 0;
+		} else if (!strcmp(clk_hw_get_name(hw), "hifi_pll")) {
+			if (readl(cntlbase + (u64)(4*4)) == AXG_HIFI_PLL_CNTL5)
+				first_set = 0;
+		} else {
+			if (readl(cntlbase + (u64)(4*4)) == GXL_GP0_CNTL5)
+				first_set = 0;
+		}
+	}
+
+	parent = clk_get_parent(hw->clk);
+
+	/*First init, just set minimal rate.*/
+	if (first_set)
+		rate = pll->rate_table[0].rate;
+	else {
+		rate = meson_axg_pll_recalc_rate(hw, clk_get_rate(parent));
+		rate = meson_axg_pll_round_rate(hw, rate, NULL);
+	}
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	ret = meson_axg_pll_set_rate(hw, rate, clk_get_rate(parent));
+
+	return ret;
+}
+
 static void meson_axg_pll_disable(struct clk_hw *hw)
 {
 	struct meson_clk_pll *pll = to_meson_clk_pll(hw);
@@ -297,6 +354,7 @@ const struct clk_ops meson_axg_pll_ops = {
 	.recalc_rate	= meson_axg_pll_recalc_rate,
 	.round_rate	= meson_axg_pll_round_rate,
 	.set_rate	= meson_axg_pll_set_rate,
+	.enable		= meson_axg_pll_enable,
 	.disable	= meson_axg_pll_disable,
 };
 
diff --git a/drivers/amlogic/clk/clk-mpll.c b/drivers/amlogic/clk/clk-mpll.c
index 92fca8b..179cf8d 100644
--- a/drivers/amlogic/clk/clk-mpll.c
+++ b/drivers/amlogic/clk/clk-mpll.c
@@ -140,6 +140,27 @@ static int mpll_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }
 
+static int mpll_enable(struct clk_hw *hw)
+{
+	struct meson_clk_mpll *mpll = to_meson_clk_mpll(hw);
+	struct parm *p = &mpll->sdm;
+	unsigned long reg;
+	unsigned long flags = 0;
+
+	if (mpll->lock)
+		spin_lock_irqsave(mpll->lock, flags);
+
+	reg = readl(mpll->base + p->reg_off);
+	reg = PARM_SET(1, mpll->sdm_en, reg, 1);
+	reg = PARM_SET(1, mpll->en_dds, reg, 1);
+	writel(reg, mpll->base + p->reg_off);
+
+	if (mpll->lock)
+		spin_unlock_irqrestore(mpll->lock, flags);
+
+	return 0;
+}
+
 void mpll_disable(struct clk_hw *hw)
 {
 	struct meson_clk_mpll *mpll = to_meson_clk_mpll(hw);
@@ -163,6 +184,7 @@ const struct clk_ops meson_clk_mpll_ops = {
 	.recalc_rate = mpll_recalc_rate,
 	.round_rate	= meson_clk_pll_round_rate,
 	.set_rate = mpll_set_rate,
+	.enable = mpll_enable,
 	.disable = mpll_disable,
 };
 
-- 
2.7.4