clk: axg: add pll disable op [1/1]

PD#146411: add disable op for pcie_gp0/hifi pll

1. add clks NULL pointer check.
2. add pll disable op.

Change-Id: Ib32e45e582b45f43b057e1188d83037628c84587
Signed-off-by: Qiufang Dai <qiufang.dai@amlogic.com>

drivers/amlogic/clk/axg/axg.c

@@ -811,14 +811,16 @@ static void __init axg_clkc_init(struct device_node *np)
 		axg_clk_gates[i]->reg = clk_base +
 			(u64)axg_clk_gates[i]->reg;
 
-#if 0
-	clks = kzalloc(NR_CLKS*sizeof(struct clk *), GFP_KERNEL);
+
 	if (!clks) {
-		/* pr_err("%s: alloc clks fail!", __func__); */
-		/* return -ENOMEM; */
-		return;
+		clks = kzalloc(NR_CLKS*sizeof(struct clk *), GFP_KERNEL);
+		if (!clks) {
+			/* pr_err("%s: alloc clks fail!", __func__); */
+			/* return -ENOMEM; */
+			return;
+		}
 	}
-#endif
+
 	if (NULL == clks) {
 		pr_err("%s: error: not kzalloc clks in aoclk!", __func__);
 		return;

drivers/amlogic/clk/axg/axg_ao.c

@@ -118,11 +118,13 @@ static void __init axg_aoclkc_init(struct device_node *np)
 	axg_saradc_div.reg = ao_clk_base + (u64)axg_saradc_div.reg;
 	axg_saradc_gate.reg = ao_clk_base + (u64)axg_saradc_gate.reg;
 
-	clks = kzalloc(NR_CLKS*sizeof(struct clk *), GFP_KERNEL);
 	if (!clks) {
-		/* pr_err("%s: alloc clks fail!", __func__); */
-		/* return -ENOMEM; */
-		return;
+		clks = kzalloc(NR_CLKS*sizeof(struct clk *), GFP_KERNEL);
+		if (!clks) {
+			/* pr_err("%s: alloc clks fail!", __func__); */
+			/* return -ENOMEM; */
+			return;
+		}
 	}
 
 	for (clkid = CLKID_AO_BASE; clkid < NR_CLKS; clkid++) {

drivers/amlogic/clk/axg/axg_clk-pll.c

@@ -171,6 +171,7 @@ static int meson_axg_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 	unsigned long old_rate;
 	int ret = 0;
 	u32 reg;
+	unsigned long flags = 0;
 
 	if (parent_rate == 0 || rate == 0)
 		return -EINVAL;
@@ -183,6 +184,9 @@ static int meson_axg_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 
 	p = &pll->n;
 
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
 	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")) {
@@ -256,6 +260,10 @@ static int meson_axg_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 	writel(reg & (~MESON_PLL_RESET), pll->base + p->reg_off);
 
 	ret = meson_axg_pll_wait_lock(pll, p);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
 	if (ret) {
 		pr_warn("%s: pll did not lock, trying to restore old rate %lu\n",
 			__func__, old_rate);
@@ -265,10 +273,31 @@ static int meson_axg_pll_set_rate(struct clk_hw *hw, unsigned long rate,
 	return ret;
 }
 
+static void meson_axg_pll_disable(struct clk_hw *hw)
+{
+	struct meson_clk_pll *pll = to_meson_clk_pll(hw);
+	struct parm *p = &pll->n;
+	unsigned long flags = 0;
+
+	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")) {
+		if (pll->lock)
+			spin_lock_irqsave(pll->lock, flags);
+
+		writel(readl(pll->base + p->reg_off) & (~MESON_PLL_ENABLE),
+			pll->base + p->reg_off);
+
+		if (pll->lock)
+			spin_unlock_irqrestore(pll->lock, flags);
+	}
+}
+
 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,
+	.disable	= meson_axg_pll_disable,
 };
 
 const struct clk_ops meson_axg_pll_ro_ops = {

drivers/amlogic/clk/clk-mpll.c

@@ -98,16 +98,16 @@ static int mpll_set_rate(struct clk_hw *hw, unsigned long rate,
 	old_rate = (parent_rate * SDM_MAX) / ((SDM_MAX * old_n2) + old_sdm);
 	pr_debug("%s: old_sdm: %lu old_n2: %lu old_rate: %lu\n", __func__,
 		old_sdm, old_n2, old_rate);
-
-	if (old_rate == rate)
-		return 0;
-
+/*
+ *	if (old_rate == rate)
+ *		return 0;
+ */
 	/* calculate new n2 and sdm */
 	n2 = parent_rate / rate;
 	sdm = DIV_ROUND_UP((parent_rate - n2 * rate) * SDM_MAX, rate);
 	pr_debug("%s: sdm: %lu n2: %lu rate: %lu\n", __func__, sdm, n2, rate);
 
-	if (old_n2 != n2 || old_sdm != sdm) {
+	/*if (old_n2 != n2 || old_sdm != sdm)*/ {
 		p = &mpll->sdm;
 		reg = readl(mpll->base + p->reg_off);
 		reg = PARM_SET(p->width, p->shift, reg, sdm);
@@ -140,10 +140,30 @@ static int mpll_set_rate(struct clk_hw *hw, unsigned long rate,
 	return 0;
 }
 
+void mpll_disable(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, 0);
+	reg = PARM_SET(1, mpll->en_dds, reg, 0);
+	writel(reg, mpll->base + p->reg_off);
+
+	if (mpll->lock)
+		spin_unlock_irqrestore(mpll->lock, flags);
+}
+
 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,
+	.disable = mpll_disable,
 };
 
 const struct clk_ops meson_clk_mpll_ro_ops = {