These changes are the raw update to linux-4.4.6-rt14. Kernel sources

are taken from kernel.org, and rt patch from the rt wiki download page.

During the rebasing, the following patch collided:

Force tick interrupt and get rid of softirq magic(I70131fb85).

Collisions have been removed because its logic was found on the
source already.

Change-Id: I7f57a4081d9deaa0d9ccfc41a6c8daccdee3b769
Signed-off-by: José Pekkarinen <jose.pekkarinen@nokia.com>

5 files changed, 577 insertions, 114 deletions

diff --git a/kernel/drivers/clk/st/clk-flexgen.c b/kernel/drivers/clk/st/clk-flexgen.c
index 0f8db2835..24d99594c 100644
--- a/kernel/drivers/clk/st/clk-flexgen.c
+++ b/kernel/drivers/clk/st/clk-flexgen.c
@@ -5,6 +5,7 @@
  * Author:  Maxime Coquelin <maxime.coquelin@st.com> for ST-Microelectronics.
  * License terms:  GNU General Public License (GPL), version 2  */
 
+#include <linux/clk.h>
 #include <linux/clk-provider.h>
 #include <linux/module.h>
 #include <linux/slab.h>
@@ -44,7 +45,7 @@ static int flexgen_enable(struct clk_hw *hw)
 
 	clk_gate_ops.enable(fgate_hw);
 
-	pr_debug("%s: flexgen output enabled\n", __clk_get_name(hw->clk));
+	pr_debug("%s: flexgen output enabled\n", clk_hw_get_name(hw));
 	return 0;
 }
 
@@ -58,7 +59,7 @@ static void flexgen_disable(struct clk_hw *hw)
 
 	clk_gate_ops.disable(fgate_hw);
 
-	pr_debug("%s: flexgen output disabled\n", __clk_get_name(hw->clk));
+	pr_debug("%s: flexgen output disabled\n", clk_hw_get_name(hw));
 }
 
 static int flexgen_is_enabled(struct clk_hw *hw)
@@ -108,7 +109,7 @@ static long flexgen_round_rate(struct clk_hw *hw, unsigned long rate,
 	/* Round div according to exact prate and wished rate */
 	div = clk_best_div(*prate, rate);
 
-	if (__clk_get_flags(hw->clk) & CLK_SET_RATE_PARENT) {
+	if (clk_hw_get_flags(hw) & CLK_SET_RATE_PARENT) {
 		*prate = rate * div;
 		return rate;
 	}
@@ -116,7 +117,7 @@ static long flexgen_round_rate(struct clk_hw *hw, unsigned long rate,
 	return *prate / div;
 }
 
-unsigned long flexgen_recalc_rate(struct clk_hw *hw,
+static unsigned long flexgen_recalc_rate(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct flexgen *flexgen = to_flexgen(hw);
@@ -174,7 +175,7 @@ static const struct clk_ops flexgen_ops = {
 	.set_rate = flexgen_set_rate,
 };
 
-struct clk *clk_register_flexgen(const char *name,
+static struct clk *clk_register_flexgen(const char *name,
 				const char **parent_names, u8 num_parents,
 				void __iomem *reg, spinlock_t *lock, u32 idx,
 				unsigned long flexgen_flags) {
@@ -190,7 +191,7 @@ struct clk *clk_register_flexgen(const char *name,
 
 	init.name = name;
 	init.ops = &flexgen_ops;
-	init.flags = CLK_IS_BASIC | flexgen_flags;
+	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE | flexgen_flags;
 	init.parent_names = parent_names;
 	init.num_parents = num_parents;
 
@@ -243,9 +244,9 @@ static const char ** __init flexgen_get_parents(struct device_node *np,
 						       int *num_parents)
 {
 	const char **parents;
-	int nparents, i;
+	int nparents;
 
-	nparents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	nparents = of_clk_get_parent_count(np);
 	if (WARN_ON(nparents <= 0))
 		return NULL;
 
@@ -253,14 +254,12 @@ static const char ** __init flexgen_get_parents(struct device_node *np,
 	if (!parents)
 		return NULL;
 
-	for (i = 0; i < nparents; i++)
-		parents[i] = of_clk_get_parent_name(np, i);
+	*num_parents = of_clk_parent_fill(np, parents, nparents);
 
-	*num_parents = nparents;
 	return parents;
 }
 
-void __init st_of_flexgen_setup(struct device_node *np)
+static void __init st_of_flexgen_setup(struct device_node *np)
 {
 	struct device_node *pnode;
 	void __iomem *reg;
@@ -269,6 +268,7 @@ void __init st_of_flexgen_setup(struct device_node *np)
 	int num_parents, i;
 	spinlock_t *rlock = NULL;
 	unsigned long flex_flags = 0;
+	int ret;
 
 	pnode = of_get_parent(np);
 	if (!pnode)
@@ -286,13 +286,13 @@ void __init st_of_flexgen_setup(struct device_node *np)
 	if (!clk_data)
 		goto err;
 
-	clk_data->clk_num = of_property_count_strings(np ,
-			"clock-output-names");
-	if (clk_data->clk_num <= 0) {
+	ret = of_property_count_strings(np, "clock-output-names");
+	if (ret <= 0) {
 		pr_err("%s: Failed to get number of output clocks (%d)",
 				__func__, clk_data->clk_num);
 		goto err;
 	}
+	clk_data->clk_num = ret;
 
 	clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
 			GFP_KERNEL);
diff --git a/kernel/drivers/clk/st/clkgen-fsyn.c b/kernel/drivers/clk/st/clkgen-fsyn.c
index 6ae068ab0..576cd0354 100644
--- a/kernel/drivers/clk/st/clkgen-fsyn.c
+++ b/kernel/drivers/clk/st/clkgen-fsyn.c
@@ -15,6 +15,7 @@
 
 #include <linux/slab.h>
 #include <linux/of_address.h>
+#include <linux/clk.h>
 #include <linux/clk-provider.h>
 
 #include "clkgen.h"
@@ -306,7 +307,7 @@ static const struct clkgen_quadfs_data st_fs660c32_F_416 = {
 	.get_rate	= clk_fs660c32_dig_get_rate,
 };
 
-static const struct clkgen_quadfs_data st_fs660c32_C_407 = {
+static const struct clkgen_quadfs_data st_fs660c32_C = {
 	.nrst_present = true,
 	.nrst	= { CLKGEN_FIELD(0x2f0, 0x1, 0),
 		    CLKGEN_FIELD(0x2f0, 0x1, 1),
@@ -349,7 +350,7 @@ static const struct clkgen_quadfs_data st_fs660c32_C_407 = {
 	.get_rate	= clk_fs660c32_dig_get_rate,
 };
 
-static const struct clkgen_quadfs_data st_fs660c32_D_407 = {
+static const struct clkgen_quadfs_data st_fs660c32_D = {
 	.nrst_present = true,
 	.nrst	= { CLKGEN_FIELD(0x2a0, 0x1, 0),
 		    CLKGEN_FIELD(0x2a0, 0x1, 1),
@@ -489,10 +490,10 @@ static int quadfs_pll_is_enabled(struct clk_hw *hw)
 	struct st_clk_quadfs_pll *pll = to_quadfs_pll(hw);
 	u32 npda = CLKGEN_READ(pll, npda);
 
-	return !!npda;
+	return pll->data->powerup_polarity ? !npda : !!npda;
 }
 
-int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs,
+static int clk_fs660c32_vco_get_rate(unsigned long input, struct stm_fs *fs,
 			   unsigned long *rate)
 {
 	unsigned long nd = fs->ndiv + 16; /* ndiv value */
@@ -512,14 +513,14 @@ static unsigned long quadfs_pll_fs660c32_recalc_rate(struct clk_hw *hw,
 	params.ndiv = CLKGEN_READ(pll, ndiv);
 	if (clk_fs660c32_vco_get_rate(parent_rate, &params, &rate))
 		pr_err("%s:%s error calculating rate\n",
-		       __clk_get_name(hw->clk), __func__);
+		       clk_hw_get_name(hw), __func__);
 
 	pll->ndiv = params.ndiv;
 
 	return rate;
 }
 
-int clk_fs660c32_vco_get_params(unsigned long input,
+static int clk_fs660c32_vco_get_params(unsigned long input,
 				unsigned long output, struct stm_fs *fs)
 {
 /* Formula
@@ -557,7 +558,7 @@ static long quadfs_pll_fs660c32_round_rate(struct clk_hw *hw, unsigned long rate
 		clk_fs660c32_vco_get_rate(*prate, &params, &rate);
 
 	pr_debug("%s: %s new rate %ld [sdiv=0x%x,md=0x%x,pe=0x%x,nsdiv3=%u]\n",
-		 __func__, __clk_get_name(hw->clk),
+		 __func__, clk_hw_get_name(hw),
 		 rate, (unsigned int)params.sdiv,
 		 (unsigned int)params.mdiv,
 		 (unsigned int)params.pe, (unsigned int)params.nsdiv);
@@ -580,7 +581,7 @@ static int quadfs_pll_fs660c32_set_rate(struct clk_hw *hw, unsigned long rate,
 		clk_fs660c32_vco_get_rate(parent_rate, &params, &hwrate);
 
 	pr_debug("%s: %s new rate %ld [ndiv=0x%x]\n",
-		 __func__, __clk_get_name(hw->clk),
+		 __func__, clk_hw_get_name(hw),
 		 hwrate, (unsigned int)params.ndiv);
 
 	if (!hwrate)
@@ -635,7 +636,7 @@ static struct clk * __init st_clk_register_quadfs_pll(
 
 	init.name = name;
 	init.ops = quadfs->pll_ops;
-	init.flags = CLK_IS_BASIC;
+	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
 	init.parent_names = &parent_name;
 	init.num_parents = 1;
 
@@ -744,7 +745,7 @@ static int quadfs_fsynth_enable(struct clk_hw *hw)
 	struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 	unsigned long flags = 0;
 
-	pr_debug("%s: %s\n", __func__, __clk_get_name(hw->clk));
+	pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
 
 	quadfs_fsynth_program_rate(fs);
 
@@ -769,12 +770,12 @@ static void quadfs_fsynth_disable(struct clk_hw *hw)
 	struct st_clk_quadfs_fsynth *fs = to_quadfs_fsynth(hw);
 	unsigned long flags = 0;
 
-	pr_debug("%s: %s\n", __func__, __clk_get_name(hw->clk));
+	pr_debug("%s: %s\n", __func__, clk_hw_get_name(hw));
 
 	if (fs->lock)
 		spin_lock_irqsave(fs->lock, flags);
 
-	CLKGEN_WRITE(fs, nsb[fs->chan], !fs->data->standby_polarity);
+	CLKGEN_WRITE(fs, nsb[fs->chan], fs->data->standby_polarity);
 
 	if (fs->lock)
 		spin_unlock_irqrestore(fs->lock, flags);
@@ -786,7 +787,7 @@ static int quadfs_fsynth_is_enabled(struct clk_hw *hw)
 	u32 nsb = CLKGEN_READ(fs, nsb[fs->chan]);
 
 	pr_debug("%s: %s enable bit = 0x%x\n",
-		 __func__, __clk_get_name(hw->clk), nsb);
+		 __func__, clk_hw_get_name(hw), nsb);
 
 	return fs->data->standby_polarity ? !nsb : !!nsb;
 }
@@ -945,10 +946,10 @@ static unsigned long quadfs_recalc_rate(struct clk_hw *hw,
 
 	if (clk_fs_get_rate(parent_rate, &params, &rate)) {
 		pr_err("%s:%s error calculating rate\n",
-		       __clk_get_name(hw->clk), __func__);
+		       clk_hw_get_name(hw), __func__);
 	}
 
-	pr_debug("%s:%s rate %lu\n", __clk_get_name(hw->clk), __func__, rate);
+	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
 
 	return rate;
 }
@@ -961,7 +962,7 @@ static long quadfs_round_rate(struct clk_hw *hw, unsigned long rate,
 	rate = quadfs_find_best_rate(hw, rate, *prate, &params);
 
 	pr_debug("%s: %s new rate %ld [sdiv=0x%x,md=0x%x,pe=0x%x,nsdiv3=%u]\n",
-		 __func__, __clk_get_name(hw->clk),
+		 __func__, clk_hw_get_name(hw),
 		 rate, (unsigned int)params.sdiv, (unsigned int)params.mdiv,
 			 (unsigned int)params.pe, (unsigned int)params.nsdiv);
 
@@ -1076,15 +1077,11 @@ static const struct of_device_id quadfs_of_match[] = {
 	},
 	{
 		.compatible = "st,stih407-quadfs660-C",
-		.data = &st_fs660c32_C_407
-	},
-	{
-		.compatible = "st,stih407-quadfs660-D",
-		.data = &st_fs660c32_D_407
+		.data = &st_fs660c32_C
 	},
 	{
 		.compatible = "st,stih407-quadfs660-D",
-		.data = (void *)&st_fs660c32_D_407
+		.data = &st_fs660c32_D
 	},
 	{}
 };
diff --git a/kernel/drivers/clk/st/clkgen-mux.c b/kernel/drivers/clk/st/clkgen-mux.c
index ef6514636..5dc5ce217 100644
--- a/kernel/drivers/clk/st/clkgen-mux.c
+++ b/kernel/drivers/clk/st/clkgen-mux.c
@@ -15,7 +15,9 @@
 
 #include <linux/slab.h>
 #include <linux/of_address.h>
+#include <linux/clk.h>
 #include <linux/clk-provider.h>
+#include "clkgen.h"
 
 static DEFINE_SPINLOCK(clkgena_divmux_lock);
 static DEFINE_SPINLOCK(clkgenf_lock);
@@ -24,20 +26,17 @@ static const char ** __init clkgen_mux_get_parents(struct device_node *np,
 						       int *num_parents)
 {
 	const char **parents;
-	int nparents, i;
+	int nparents;
 
-	nparents = of_count_phandle_with_args(np, "clocks", "#clock-cells");
+	nparents = of_clk_get_parent_count(np);
 	if (WARN_ON(nparents <= 0))
 		return ERR_PTR(-EINVAL);
 
-	parents = kzalloc(nparents * sizeof(const char *), GFP_KERNEL);
+	parents = kcalloc(nparents, sizeof(const char *), GFP_KERNEL);
 	if (!parents)
 		return ERR_PTR(-ENOMEM);
 
-	for (i = 0; i < nparents; i++)
-		parents[i] = of_clk_get_parent_name(np, i);
-
-	*num_parents = nparents;
+	*num_parents = of_clk_parent_fill(np, parents, nparents);
 	return parents;
 }
 
@@ -131,7 +130,7 @@ static int clkgena_divmux_is_enabled(struct clk_hw *hw)
 	return (s8)clk_mux_ops.get_parent(mux_hw) > 0;
 }
 
-u8 clkgena_divmux_get_parent(struct clk_hw *hw)
+static u8 clkgena_divmux_get_parent(struct clk_hw *hw)
 {
 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
 	struct clk_hw *mux_hw = &genamux->mux.hw;
@@ -141,7 +140,7 @@ u8 clkgena_divmux_get_parent(struct clk_hw *hw)
 	genamux->muxsel = clk_mux_ops.get_parent(mux_hw);
 	if ((s8)genamux->muxsel < 0) {
 		pr_debug("%s: %s: Invalid parent, setting to default.\n",
-		      __func__, __clk_get_name(hw->clk));
+		      __func__, clk_hw_get_name(hw));
 		genamux->muxsel = 0;
 	}
 
@@ -168,7 +167,7 @@ static int clkgena_divmux_set_parent(struct clk_hw *hw, u8 index)
 	return 0;
 }
 
-unsigned long clkgena_divmux_recalc_rate(struct clk_hw *hw,
+static unsigned long clkgena_divmux_recalc_rate(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgena_divmux *genamux = to_clkgena_divmux(hw);
@@ -215,7 +214,7 @@ static const struct clk_ops clkgena_divmux_ops = {
 /**
  * clk_register_genamux - register a genamux clock with the clock framework
  */
-struct clk *clk_register_genamux(const char *name,
+static struct clk * __init clk_register_genamux(const char *name,
 				const char **parent_names, u8 num_parents,
 				void __iomem *reg,
 				const struct clkgena_divmux_data *muxdata,
@@ -237,7 +236,7 @@ struct clk *clk_register_genamux(const char *name,
 
 	init.name = name;
 	init.ops = &clkgena_divmux_ops;
-	init.flags = CLK_IS_BASIC;
+	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
 	init.parent_names = parent_names;
 	init.num_parents = num_parents;
 
@@ -369,11 +368,10 @@ static const struct of_device_id clkgena_divmux_of_match[] = {
 	{}
 };
 
-static void __iomem * __init clkgen_get_register_base(
-				struct device_node *np)
+static void __iomem * __init clkgen_get_register_base(struct device_node *np)
 {
 	struct device_node *pnode;
-	void __iomem *reg = NULL;
+	void __iomem *reg;
 
 	pnode = of_get_parent(np);
 	if (!pnode)
@@ -385,7 +383,7 @@ static void __iomem * __init clkgen_get_register_base(
 	return reg;
 }
 
-void __init st_of_clkgena_divmux_setup(struct device_node *np)
+static void __init st_of_clkgena_divmux_setup(struct device_node *np)
 {
 	const struct of_device_id *match;
 	const struct clkgena_divmux_data *data;
@@ -398,7 +396,7 @@ void __init st_of_clkgena_divmux_setup(struct device_node *np)
 	if (WARN_ON(!match))
 		return;
 
-	data = (struct clkgena_divmux_data *)match->data;
+	data = match->data;
 
 	reg = clkgen_get_register_base(np);
 	if (!reg)
@@ -406,18 +404,18 @@ void __init st_of_clkgena_divmux_setup(struct device_node *np)
 
 	parents = clkgen_mux_get_parents(np, &num_parents);
 	if (IS_ERR(parents))
-		return;
+		goto err_parents;
 
 	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
 	if (!clk_data)
-		goto err;
+		goto err_alloc;
 
 	clk_data->clk_num = data->num_outputs;
-	clk_data->clks = kzalloc(clk_data->clk_num * sizeof(struct clk *),
+	clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
 				 GFP_KERNEL);
 
 	if (!clk_data->clks)
-		goto err;
+		goto err_alloc_clks;
 
 	for (i = 0; i < clk_data->clk_num; i++) {
 		struct clk *clk;
@@ -447,11 +445,13 @@ void __init st_of_clkgena_divmux_setup(struct device_node *np)
 	of_clk_add_provider(np, of_clk_src_onecell_get, clk_data);
 	return;
 err:
-	if (clk_data)
-		kfree(clk_data->clks);
-
+	kfree(clk_data->clks);
+err_alloc_clks:
 	kfree(clk_data);
+err_alloc:
 	kfree(parents);
+err_parents:
+	iounmap(reg);
 }
 CLK_OF_DECLARE(clkgenadivmux, "st,clkgena-divmux", st_of_clkgena_divmux_setup);
 
@@ -485,13 +485,13 @@ static const struct of_device_id clkgena_prediv_of_match[] = {
 	{}
 };
 
-void __init st_of_clkgena_prediv_setup(struct device_node *np)
+static void __init st_of_clkgena_prediv_setup(struct device_node *np)
 {
 	const struct of_device_id *match;
 	void __iomem *reg;
 	const char *parent_name, *clk_name;
 	struct clk *clk;
-	struct clkgena_prediv_data *data;
+	const struct clkgena_prediv_data *data;
 
 	match = of_match_node(clkgena_prediv_of_match, np);
 	if (!match) {
@@ -499,7 +499,7 @@ void __init st_of_clkgena_prediv_setup(struct device_node *np)
 		return;
 	}
 
-	data = (struct clkgena_prediv_data *)match->data;
+	data = match->data;
 
 	reg = clkgen_get_register_base(np);
 	if (!reg)
@@ -507,17 +507,18 @@ void __init st_of_clkgena_prediv_setup(struct device_node *np)
 
 	parent_name = of_clk_get_parent_name(np, 0);
 	if (!parent_name)
-		return;
+		goto err;
 
 	if (of_property_read_string_index(np, "clock-output-names",
 					  0, &clk_name))
-		return;
+		goto err;
 
-	clk = clk_register_divider_table(NULL, clk_name, parent_name, 0,
+	clk = clk_register_divider_table(NULL, clk_name, parent_name,
+					 CLK_GET_RATE_NOCACHE,
 					 reg + data->offset, data->shift, 1,
 					 0, data->table, NULL);
 	if (IS_ERR(clk))
-		return;
+		goto err;
 
 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
 	pr_debug("%s: parent %s rate %u\n",
@@ -526,6 +527,8 @@ void __init st_of_clkgena_prediv_setup(struct device_node *np)
 		(unsigned int)clk_get_rate(clk));
 
 	return;
+err:
+	iounmap(reg);
 }
 CLK_OF_DECLARE(clkgenaprediv, "st,clkgena-prediv", st_of_clkgena_prediv_setup);
 
@@ -574,6 +577,7 @@ static struct clkgen_mux_data stih415_a9_mux_data = {
 	.offset = 0,
 	.shift = 1,
 	.width = 2,
+	.lock = &clkgen_a9_lock,
 };
 static struct clkgen_mux_data stih416_a9_mux_data = {
 	.offset = 0,
@@ -584,6 +588,7 @@ static struct clkgen_mux_data stih407_a9_mux_data = {
 	.offset = 0x1a4,
 	.shift = 0,
 	.width = 2,
+	.lock = &clkgen_a9_lock,
 };
 
 static const struct of_device_id mux_of_match[] = {
@@ -622,14 +627,14 @@ static const struct of_device_id mux_of_match[] = {
 	{}
 };
 
-void __init st_of_clkgen_mux_setup(struct device_node *np)
+static void __init st_of_clkgen_mux_setup(struct device_node *np)
 {
 	const struct of_device_id *match;
 	struct clk *clk;
 	void __iomem *reg;
 	const char **parents;
 	int num_parents;
-	struct clkgen_mux_data *data;
+	const struct clkgen_mux_data *data;
 
 	match = of_match_node(mux_of_match, np);
 	if (!match) {
@@ -637,7 +642,7 @@ void __init st_of_clkgen_mux_setup(struct device_node *np)
 		return;
 	}
 
-	data = (struct clkgen_mux_data *)match->data;
+	data = match->data;
 
 	reg = of_iomap(np, 0);
 	if (!reg) {
@@ -649,7 +654,7 @@ void __init st_of_clkgen_mux_setup(struct device_node *np)
 	if (IS_ERR(parents)) {
 		pr_err("%s: Failed to get parents (%ld)\n",
 				__func__, PTR_ERR(parents));
-		return;
+		goto err_parents;
 	}
 
 	clk = clk_register_mux(NULL, np->name, parents, num_parents,
@@ -665,12 +670,14 @@ void __init st_of_clkgen_mux_setup(struct device_node *np)
 			__clk_get_name(clk_get_parent(clk)),
 			(unsigned int)clk_get_rate(clk));
 
+	kfree(parents);
 	of_clk_add_provider(np, of_clk_src_simple_get, clk);
+	return;
 
 err:
 	kfree(parents);
-
-	return;
+err_parents:
+	iounmap(reg);
 }
 CLK_OF_DECLARE(clkgen_mux, "st,clkgen-mux", st_of_clkgen_mux_setup);
 
@@ -699,19 +706,19 @@ static const struct of_device_id vcc_of_match[] = {
 	{}
 };
 
-void __init st_of_clkgen_vcc_setup(struct device_node *np)
+static void __init st_of_clkgen_vcc_setup(struct device_node *np)
 {
 	const struct of_device_id *match;
 	void __iomem *reg;
 	const char **parents;
 	int num_parents, i;
 	struct clk_onecell_data *clk_data;
-	struct clkgen_vcc_data *data;
+	const struct clkgen_vcc_data *data;
 
 	match = of_match_node(vcc_of_match, np);
 	if (WARN_ON(!match))
 		return;
-	data = (struct clkgen_vcc_data *)match->data;
+	data = match->data;
 
 	reg = of_iomap(np, 0);
 	if (!reg)
@@ -719,18 +726,18 @@ void __init st_of_clkgen_vcc_setup(struct device_node *np)
 
 	parents = clkgen_mux_get_parents(np, &num_parents);
 	if (IS_ERR(parents))
-		return;
+		goto err_parents;
 
 	clk_data = kzalloc(sizeof(*clk_data), GFP_KERNEL);
 	if (!clk_data)
-		goto err;
+		goto err_alloc;
 
 	clk_data->clk_num = VCC_MAX_CHANNELS;
-	clk_data->clks = kzalloc(clk_data->clk_num * sizeof(struct clk *),
+	clk_data->clks = kcalloc(clk_data->clk_num, sizeof(struct clk *),
 				 GFP_KERNEL);
 
 	if (!clk_data->clks)
-		goto err;
+		goto err_alloc_clks;
 
 	for (i = 0; i < clk_data->clk_num; i++) {
 		struct clk *clk;
@@ -749,21 +756,21 @@ void __init st_of_clkgen_vcc_setup(struct device_node *np)
 		if (*clk_name == '\0')
 			continue;
 
-		gate = kzalloc(sizeof(struct clk_gate), GFP_KERNEL);
+		gate = kzalloc(sizeof(*gate), GFP_KERNEL);
 		if (!gate)
-			break;
+			goto err;
 
-		div = kzalloc(sizeof(struct clk_divider), GFP_KERNEL);
+		div = kzalloc(sizeof(*div), GFP_KERNEL);
 		if (!div) {
 			kfree(gate);
-			break;
+			goto err;
 		}
 
-		mux = kzalloc(sizeof(struct clk_mux), GFP_KERNEL);
+		mux = kzalloc(sizeof(*mux), GFP_KERNEL);
 		if (!mux) {
 			kfree(gate);
 			kfree(div);
-			break;
+			goto err;
 		}
 
 		gate->reg = reg + VCC_GATE_OFFSET;
@@ -786,7 +793,8 @@ void __init st_of_clkgen_vcc_setup(struct device_node *np)
 					     &mux->hw, &clk_mux_ops,
 					     &div->hw, &clk_divider_ops,
 					     &gate->hw, &clk_gate_ops,
-					     data->clk_flags);
+					     data->clk_flags |
+					     CLK_GET_RATE_NOCACHE);
 		if (IS_ERR(clk)) {
 			kfree(gate);
 			kfree(div);
@@ -821,10 +829,12 @@ err:
 		kfree(container_of(composite->mux_hw, struct clk_mux, hw));
 	}
 
-	if (clk_data)
-		kfree(clk_data->clks);
-
+	kfree(clk_data->clks);
+err_alloc_clks:
 	kfree(clk_data);
+err_alloc:
 	kfree(parents);
+err_parents:
+	iounmap(reg);
 }
 CLK_OF_DECLARE(clkgen_vcc, "st,clkgen-vcc", st_of_clkgen_vcc_setup);
diff --git a/kernel/drivers/clk/st/clkgen-pll.c b/kernel/drivers/clk/st/clkgen-pll.c
index d204ba85d..38f6f3a90 100644
--- a/kernel/drivers/clk/st/clkgen-pll.c
+++ b/kernel/drivers/clk/st/clkgen-pll.c
@@ -16,11 +16,14 @@
 
 #include <linux/slab.h>
 #include <linux/of_address.h>
+#include <linux/clk.h>
 #include <linux/clk-provider.h>
+#include <linux/iopoll.h>
 
 #include "clkgen.h"
 
 static DEFINE_SPINLOCK(clkgena_c32_odf_lock);
+DEFINE_SPINLOCK(clkgen_a9_lock);
 
 /*
  * Common PLL configuration register bits for PLL800 and PLL1600 C65
@@ -37,30 +40,46 @@ static DEFINE_SPINLOCK(clkgena_c32_odf_lock);
 #define C32_IDF_MASK (0x7)
 #define C32_ODF_MASK (0x3f)
 #define C32_LDF_MASK (0x7f)
+#define C32_CP_MASK (0x1f)
 
 #define C32_MAX_ODFS (4)
 
+/*
+ * PLL configuration register bits for PLL4600 C28
+ */
+#define C28_NDIV_MASK (0xff)
+#define C28_IDF_MASK (0x7)
+#define C28_ODF_MASK (0x3f)
+
 struct clkgen_pll_data {
 	struct clkgen_field pdn_status;
+	struct clkgen_field pdn_ctrl;
 	struct clkgen_field locked_status;
 	struct clkgen_field mdiv;
 	struct clkgen_field ndiv;
 	struct clkgen_field pdiv;
 	struct clkgen_field idf;
 	struct clkgen_field ldf;
+	struct clkgen_field cp;
 	unsigned int num_odfs;
 	struct clkgen_field odf[C32_MAX_ODFS];
 	struct clkgen_field odf_gate[C32_MAX_ODFS];
+	bool switch2pll_en;
+	struct clkgen_field switch2pll;
+	spinlock_t *lock;
 	const struct clk_ops *ops;
 };
 
 static const struct clk_ops st_pll1600c65_ops;
 static const struct clk_ops st_pll800c65_ops;
 static const struct clk_ops stm_pll3200c32_ops;
+static const struct clk_ops stm_pll3200c32_a9_ops;
 static const struct clk_ops st_pll1200c32_ops;
+static const struct clk_ops stm_pll4600c28_ops;
 
 static const struct clkgen_pll_data st_pll1600c65_ax = {
 	.pdn_status	= CLKGEN_FIELD(0x0, 0x1,			19),
+	.pdn_ctrl	= CLKGEN_FIELD(0x10,	0x1,			0),
 	.locked_status	= CLKGEN_FIELD(0x0, 0x1,			31),
 	.mdiv		= CLKGEN_FIELD(0x0, C65_MDIV_PLL1600_MASK,	0),
 	.ndiv		= CLKGEN_FIELD(0x0, C65_NDIV_MASK,		8),
@@ -69,6 +88,7 @@ static const struct clkgen_pll_data st_pll1600c65_ax = {
 
 static const struct clkgen_pll_data st_pll800c65_ax = {
 	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			19),
+	.pdn_ctrl	= CLKGEN_FIELD(0xC,	0x1,			1),
 	.locked_status	= CLKGEN_FIELD(0x0,	0x1,			31),
 	.mdiv		= CLKGEN_FIELD(0x0,	C65_MDIV_PLL800_MASK,	0),
 	.ndiv		= CLKGEN_FIELD(0x0,	C65_NDIV_MASK,		8),
@@ -78,6 +98,7 @@ static const struct clkgen_pll_data st_pll800c65_ax = {
 
 static const struct clkgen_pll_data st_pll3200c32_a1x_0 = {
 	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			31),
+	.pdn_ctrl	= CLKGEN_FIELD(0x18,	0x1,			0),
 	.locked_status	= CLKGEN_FIELD(0x4,	0x1,			31),
 	.ndiv		= CLKGEN_FIELD(0x0,	C32_NDIV_MASK,		0x0),
 	.idf		= CLKGEN_FIELD(0x4,	C32_IDF_MASK,		0x0),
@@ -95,6 +116,7 @@ static const struct clkgen_pll_data st_pll3200c32_a1x_0 = {
 
 static const struct clkgen_pll_data st_pll3200c32_a1x_1 = {
 	.pdn_status	= CLKGEN_FIELD(0xC,	0x1,			31),
+	.pdn_ctrl	= CLKGEN_FIELD(0x18,	0x1,			1),
 	.locked_status	= CLKGEN_FIELD(0x10,	0x1,			31),
 	.ndiv		= CLKGEN_FIELD(0xC,	C32_NDIV_MASK,		0x0),
 	.idf		= CLKGEN_FIELD(0x10,	C32_IDF_MASK,		0x0),
@@ -113,6 +135,7 @@ static const struct clkgen_pll_data st_pll3200c32_a1x_1 = {
 /* 415 specific */
 static const struct clkgen_pll_data st_pll3200c32_a9_415 = {
 	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			0),
+	.pdn_ctrl	= CLKGEN_FIELD(0x0,	0x1,			0),
 	.locked_status	= CLKGEN_FIELD(0x6C,	0x1,			0),
 	.ndiv		= CLKGEN_FIELD(0x0,	C32_NDIV_MASK,		9),
 	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		22),
@@ -124,6 +147,7 @@ static const struct clkgen_pll_data st_pll3200c32_a9_415 = {
 
 static const struct clkgen_pll_data st_pll3200c32_ddr_415 = {
 	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			0),
+	.pdn_ctrl	= CLKGEN_FIELD(0x0,	0x1,			0),
 	.locked_status	= CLKGEN_FIELD(0x100,	0x1,			0),
 	.ndiv		= CLKGEN_FIELD(0x8,	C32_NDIV_MASK,		0),
 	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		25),
@@ -136,7 +160,8 @@ static const struct clkgen_pll_data st_pll3200c32_ddr_415 = {
 };
 
 static const struct clkgen_pll_data st_pll1200c32_gpu_415 = {
-	.pdn_status	= CLKGEN_FIELD(0x144,	0x1,			3),
+	.pdn_status	= CLKGEN_FIELD(0x4,	0x1,			0),
+	.pdn_ctrl	= CLKGEN_FIELD(0x4,	0x1,			0),
 	.locked_status	= CLKGEN_FIELD(0x168,	0x1,			0),
 	.ldf		= CLKGEN_FIELD(0x0,	C32_LDF_MASK,		3),
 	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		0),
@@ -148,6 +173,7 @@ static const struct clkgen_pll_data st_pll1200c32_gpu_415 = {
 /* 416 specific */
 static const struct clkgen_pll_data st_pll3200c32_a9_416 = {
 	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			0),
+	.pdn_ctrl	= CLKGEN_FIELD(0x0,	0x1,			0),
 	.locked_status	= CLKGEN_FIELD(0x6C,	0x1,			0),
 	.ndiv		= CLKGEN_FIELD(0x8,	C32_NDIV_MASK,		0),
 	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		25),
@@ -159,6 +185,7 @@ static const struct clkgen_pll_data st_pll3200c32_a9_416 = {
 
 static const struct clkgen_pll_data st_pll3200c32_ddr_416 = {
 	.pdn_status	= CLKGEN_FIELD(0x0,	0x1,			0),
+	.pdn_ctrl	= CLKGEN_FIELD(0x0,	0x1,			0),
 	.locked_status	= CLKGEN_FIELD(0x10C,	0x1,			0),
 	.ndiv		= CLKGEN_FIELD(0x8,	C32_NDIV_MASK,		0),
 	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		25),
@@ -172,6 +199,7 @@ static const struct clkgen_pll_data st_pll3200c32_ddr_416 = {
 
 static const struct clkgen_pll_data st_pll1200c32_gpu_416 = {
 	.pdn_status	= CLKGEN_FIELD(0x8E4,	0x1,			3),
+	.pdn_ctrl	= CLKGEN_FIELD(0x8E4,	0x1,			3),
 	.locked_status	= CLKGEN_FIELD(0x90C,	0x1,			0),
 	.ldf		= CLKGEN_FIELD(0x0,	C32_LDF_MASK,		3),
 	.idf		= CLKGEN_FIELD(0x0,	C32_IDF_MASK,		0),
@@ -183,6 +211,7 @@ static const struct clkgen_pll_data st_pll1200c32_gpu_416 = {
 static const struct clkgen_pll_data st_pll3200c32_407_a0 = {
 	/* 407 A0 */
 	.pdn_status	= CLKGEN_FIELD(0x2a0,	0x1,			8),
+	.pdn_ctrl	= CLKGEN_FIELD(0x2a0,	0x1,			8),
 	.locked_status	= CLKGEN_FIELD(0x2a0,	0x1,			24),
 	.ndiv		= CLKGEN_FIELD(0x2a4,	C32_NDIV_MASK,		16),
 	.idf		= CLKGEN_FIELD(0x2a4,	C32_IDF_MASK,		0x0),
@@ -192,9 +221,10 @@ static const struct clkgen_pll_data st_pll3200c32_407_a0 = {
 	.ops		= &stm_pll3200c32_ops,
 };
 
-static const struct clkgen_pll_data st_pll3200c32_407_c0_0 = {
+static const struct clkgen_pll_data st_pll3200c32_cx_0 = {
 	/* 407 C0 PLL0 */
 	.pdn_status	= CLKGEN_FIELD(0x2a0,	0x1,			8),
+	.pdn_ctrl	= CLKGEN_FIELD(0x2a0,	0x1,			8),
 	.locked_status	= CLKGEN_FIELD(0x2a0,	0x1,			24),
 	.ndiv		= CLKGEN_FIELD(0x2a4,	C32_NDIV_MASK,		16),
 	.idf		= CLKGEN_FIELD(0x2a4,	C32_IDF_MASK,		0x0),
@@ -204,9 +234,10 @@ static const struct clkgen_pll_data st_pll3200c32_407_c0_0 = {
 	.ops		= &stm_pll3200c32_ops,
 };
 
-static const struct clkgen_pll_data st_pll3200c32_407_c0_1 = {
+static const struct clkgen_pll_data st_pll3200c32_cx_1 = {
 	/* 407 C0 PLL1 */
 	.pdn_status	= CLKGEN_FIELD(0x2c8,	0x1,			8),
+	.pdn_ctrl	= CLKGEN_FIELD(0x2c8,	0x1,			8),
 	.locked_status	= CLKGEN_FIELD(0x2c8,	0x1,			24),
 	.ndiv		= CLKGEN_FIELD(0x2cc,	C32_NDIV_MASK,		16),
 	.idf		= CLKGEN_FIELD(0x2cc,	C32_IDF_MASK,		0x0),
@@ -219,13 +250,34 @@ static const struct clkgen_pll_data st_pll3200c32_407_c0_1 = {
 static const struct clkgen_pll_data st_pll3200c32_407_a9 = {
 	/* 407 A9 */
 	.pdn_status	= CLKGEN_FIELD(0x1a8,	0x1,			0),
+	.pdn_ctrl	= CLKGEN_FIELD(0x1a8,	0x1,			0),
 	.locked_status	= CLKGEN_FIELD(0x87c,	0x1,			0),
 	.ndiv		= CLKGEN_FIELD(0x1b0,	C32_NDIV_MASK,		0),
 	.idf		= CLKGEN_FIELD(0x1a8,	C32_IDF_MASK,		25),
 	.num_odfs = 1,
 	.odf		= { CLKGEN_FIELD(0x1b0, C32_ODF_MASK,		8) },
 	.odf_gate	= { CLKGEN_FIELD(0x1ac, 0x1,			28) },
-	.ops		= &stm_pll3200c32_ops,
+	.switch2pll_en	= true,
+	.cp		= CLKGEN_FIELD(0x1a8,	C32_CP_MASK,		1),
+	.switch2pll	= CLKGEN_FIELD(0x1a4,	0x1,			1),
+	.lock = &clkgen_a9_lock,
+	.ops		= &stm_pll3200c32_a9_ops,
+};
+
+static struct clkgen_pll_data st_pll4600c28_418_a9 = {
+	/* 418 A9 */
+	.pdn_status	= CLKGEN_FIELD(0x1a8,	0x1,			0),
+	.pdn_ctrl	= CLKGEN_FIELD(0x1a8,	0x1,			0),
+	.locked_status	= CLKGEN_FIELD(0x87c,	0x1,			0),
+	.ndiv		= CLKGEN_FIELD(0x1b0,	C28_NDIV_MASK,		0),
+	.idf		= CLKGEN_FIELD(0x1a8,	C28_IDF_MASK,		25),
+	.num_odfs = 1,
+	.odf		= { CLKGEN_FIELD(0x1b0, C28_ODF_MASK,		8) },
+	.odf_gate	= { CLKGEN_FIELD(0x1ac, 0x1,			28) },
+	.switch2pll_en	= true,
+	.switch2pll	= CLKGEN_FIELD(0x1a4,	0x1,			1),
+	.lock		= &clkgen_a9_lock,
+	.ops		= &stm_pll4600c28_ops,
 };
 
 /**
@@ -251,10 +303,26 @@ struct clkgen_pll {
 	struct clk_hw		hw;
 	struct clkgen_pll_data	*data;
 	void __iomem		*regs_base;
+	spinlock_t	*lock;
+
+	u32 ndiv;
+	u32 idf;
+	u32 odf;
+	u32 cp;
 };
 
 #define to_clkgen_pll(_hw) container_of(_hw, struct clkgen_pll, hw)
 
+struct stm_pll {
+	unsigned long mdiv;
+	unsigned long ndiv;
+	unsigned long pdiv;
+	unsigned long odf;
+	unsigned long idf;
+	unsigned long ldf;
+	unsigned long cp;
+};
+
 static int clkgen_pll_is_locked(struct clk_hw *hw)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -270,7 +338,79 @@ static int clkgen_pll_is_enabled(struct clk_hw *hw)
 	return !poweroff;
 }
 
-unsigned long recalc_stm_pll800c65(struct clk_hw *hw,
+static int __clkgen_pll_enable(struct clk_hw *hw)
+{
+	struct clkgen_pll *pll = to_clkgen_pll(hw);
+	void __iomem *base =  pll->regs_base;
+	struct clkgen_field *field = &pll->data->locked_status;
+	int ret = 0;
+	u32 reg;
+
+	if (clkgen_pll_is_enabled(hw))
+		return 0;
+
+	CLKGEN_WRITE(pll, pdn_ctrl, 0);
+
+	ret = readl_relaxed_poll_timeout(base + field->offset, reg,
+			!!((reg >> field->shift) & field->mask),  0, 10000);
+
+	if (!ret) {
+		if (pll->data->switch2pll_en)
+			CLKGEN_WRITE(pll, switch2pll, 0);
+
+		pr_debug("%s:%s enabled\n", __clk_get_name(hw->clk), __func__);
+	}
+
+	return ret;
+}
+
+static int clkgen_pll_enable(struct clk_hw *hw)
+{
+	struct clkgen_pll *pll = to_clkgen_pll(hw);
+	unsigned long flags = 0;
+	int ret = 0;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	ret = __clkgen_pll_enable(hw);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	return ret;
+}
+
+static void __clkgen_pll_disable(struct clk_hw *hw)
+{
+	struct clkgen_pll *pll = to_clkgen_pll(hw);
+
+	if (!clkgen_pll_is_enabled(hw))
+		return;
+
+	if (pll->data->switch2pll_en)
+		CLKGEN_WRITE(pll, switch2pll, 1);
+
+	CLKGEN_WRITE(pll, pdn_ctrl, 1);
+
+	pr_debug("%s:%s disabled\n", __clk_get_name(hw->clk), __func__);
+}
+
+static void clkgen_pll_disable(struct clk_hw *hw)
+{
+	struct clkgen_pll *pll = to_clkgen_pll(hw);
+	unsigned long flags = 0;
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	__clkgen_pll_disable(hw);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+}
+
+static unsigned long recalc_stm_pll800c65(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -291,13 +431,13 @@ unsigned long recalc_stm_pll800c65(struct clk_hw *hw,
 	res = (uint64_t)2 * (uint64_t)parent_rate * (uint64_t)ndiv;
 	rate = (unsigned long)div64_u64(res, mdiv * (1 << pdiv));
 
-	pr_debug("%s:%s rate %lu\n", __clk_get_name(hw->clk), __func__, rate);
+	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
 
 	return rate;
 
 }
 
-unsigned long recalc_stm_pll1600c65(struct clk_hw *hw,
+static unsigned long recalc_stm_pll1600c65(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -316,12 +456,73 @@ unsigned long recalc_stm_pll1600c65(struct clk_hw *hw,
 	/* Note: input is divided by 1000 to avoid overflow */
 	rate = ((2 * (parent_rate / 1000) * ndiv) / mdiv) * 1000;
 
-	pr_debug("%s:%s rate %lu\n", __clk_get_name(hw->clk), __func__, rate);
+	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
 
 	return rate;
 }
 
-unsigned long recalc_stm_pll3200c32(struct clk_hw *hw,
+static int clk_pll3200c32_get_params(unsigned long input, unsigned long output,
+			  struct stm_pll *pll)
+{
+	unsigned long i, n;
+	unsigned long deviation = ~0;
+	unsigned long new_freq;
+	long new_deviation;
+	/* Charge pump table: highest ndiv value for cp=6 to 25 */
+	static const unsigned char cp_table[] = {
+		48, 56, 64, 72, 80, 88, 96, 104, 112, 120,
+		128, 136, 144, 152, 160, 168, 176, 184, 192
+	};
+
+	/* Output clock range: 800Mhz to 1600Mhz */
+	if (output < 800000000 || output > 1600000000)
+		return -EINVAL;
+
+	input /= 1000;
+	output /= 1000;
+
+	for (i = 1; i <= 7 && deviation; i++) {
+		n = i * output / (2 * input);
+
+		/* Checks */
+		if (n < 8)
+			continue;
+		if (n > 200)
+			break;
+
+		new_freq = (input * 2 * n) / i;
+
+		new_deviation = abs(new_freq - output);
+
+		if (!new_deviation || new_deviation < deviation) {
+			pll->idf  = i;
+			pll->ndiv = n;
+			deviation = new_deviation;
+		}
+	}
+
+	if (deviation == ~0) /* No solution found */
+		return -EINVAL;
+
+	/* Computing recommended charge pump value */
+	for (pll->cp = 6; pll->ndiv > cp_table[pll->cp-6]; (pll->cp)++)
+		;
+
+	return 0;
+}
+
+static int clk_pll3200c32_get_rate(unsigned long input, struct stm_pll *pll,
+			unsigned long *rate)
+{
+	if (!pll->idf)
+		pll->idf = 1;
+
+	*rate = ((2 * (input / 1000) * pll->ndiv) / pll->idf) * 1000;
+
+	return 0;
+}
+
+static unsigned long recalc_stm_pll3200c32(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -338,12 +539,76 @@ unsigned long recalc_stm_pll3200c32(struct clk_hw *hw,
 		/* Note: input is divided to avoid overflow */
 		rate = ((2 * (parent_rate/1000) * ndiv) / idf) * 1000;
 
-	pr_debug("%s:%s rate %lu\n", __clk_get_name(hw->clk), __func__, rate);
+	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
+
+	return rate;
+}
+
+static long round_rate_stm_pll3200c32(struct clk_hw *hw, unsigned long rate,
+		unsigned long *prate)
+{
+	struct stm_pll params;
+
+	if (!clk_pll3200c32_get_params(*prate, rate, &params))
+		clk_pll3200c32_get_rate(*prate, &params, &rate);
+	else {
+		pr_debug("%s: %s rate %ld Invalid\n", __func__,
+			 __clk_get_name(hw->clk), rate);
+		return 0;
+	}
+
+	pr_debug("%s: %s new rate %ld [ndiv=%u] [idf=%u]\n",
+		 __func__, __clk_get_name(hw->clk),
+		 rate, (unsigned int)params.ndiv,
+		 (unsigned int)params.idf);
 
 	return rate;
 }
 
-unsigned long recalc_stm_pll1200c32(struct clk_hw *hw,
+static int set_rate_stm_pll3200c32(struct clk_hw *hw, unsigned long rate,
+				unsigned long parent_rate)
+{
+	struct clkgen_pll *pll = to_clkgen_pll(hw);
+	struct stm_pll params;
+	long hwrate = 0;
+	unsigned long flags = 0;
+
+	if (!rate || !parent_rate)
+		return -EINVAL;
+
+	if (!clk_pll3200c32_get_params(parent_rate, rate, &params))
+		clk_pll3200c32_get_rate(parent_rate, &params, &hwrate);
+
+	pr_debug("%s: %s new rate %ld [ndiv=0x%x] [idf=0x%x]\n",
+		 __func__, __clk_get_name(hw->clk),
+		 hwrate, (unsigned int)params.ndiv,
+		 (unsigned int)params.idf);
+
+	if (!hwrate)
+		return -EINVAL;
+
+	pll->ndiv = params.ndiv;
+	pll->idf = params.idf;
+	pll->cp = params.cp;
+
+	__clkgen_pll_disable(hw);
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	CLKGEN_WRITE(pll, ndiv, pll->ndiv);
+	CLKGEN_WRITE(pll, idf, pll->idf);
+	CLKGEN_WRITE(pll, cp, pll->cp);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	__clkgen_pll_enable(hw);
+
+	return 0;
+}
+
+static unsigned long recalc_stm_pll1200c32(struct clk_hw *hw,
 		unsigned long parent_rate)
 {
 	struct clkgen_pll *pll = to_clkgen_pll(hw);
@@ -365,35 +630,218 @@ unsigned long recalc_stm_pll1200c32(struct clk_hw *hw,
 	/* Note: input is divided by 1000 to avoid overflow */
 	rate = (((parent_rate / 1000) * ldf) / (odf * idf)) * 1000;
 
+	pr_debug("%s:%s rate %lu\n", clk_hw_get_name(hw), __func__, rate);
+
+	return rate;
+}
+
+/* PLL output structure
+ * FVCO >> /2 >> FVCOBY2 (no output)
+ *                 |> Divider (ODF) >> PHI
+ *
+ * FVCOby2 output = (input * 2 * NDIV) / IDF (assuming FRAC_CONTROL==L)
+ *
+ * Rules:
+ *   4Mhz <= INFF input <= 350Mhz
+ *   4Mhz <= INFIN (INFF / IDF) <= 50Mhz
+ *   19.05Mhz <= FVCOby2 output (PHI w ODF=1) <= 3000Mhz
+ *   1 <= i (register/dec value for IDF) <= 7
+ *   8 <= n (register/dec value for NDIV) <= 246
+ */
+
+static int clk_pll4600c28_get_params(unsigned long input, unsigned long output,
+			  struct stm_pll *pll)
+{
+
+	unsigned long i, infin, n;
+	unsigned long deviation = ~0;
+	unsigned long new_freq, new_deviation;
+
+	/* Output clock range: 19Mhz to 3000Mhz */
+	if (output < 19000000 || output > 3000000000u)
+		return -EINVAL;
+
+	/* For better jitter, IDF should be smallest and NDIV must be maximum */
+	for (i = 1; i <= 7 && deviation; i++) {
+		/* INFIN checks */
+		infin = input / i;
+		if (infin < 4000000 || infin > 50000000)
+			continue;	/* Invalid case */
+
+		n = output / (infin * 2);
+		if (n < 8 || n > 246)
+			continue;	/* Invalid case */
+		if (n < 246)
+			n++;	/* To work around 'y' when n=x.y */
+
+		for (; n >= 8 && deviation; n--) {
+			new_freq = infin * 2 * n;
+			if (new_freq < output)
+				break;	/* Optimization: shorting loop */
+
+			new_deviation = new_freq - output;
+			if (!new_deviation || new_deviation < deviation) {
+				pll->idf  = i;
+				pll->ndiv = n;
+				deviation = new_deviation;
+			}
+		}
+	}
+
+	if (deviation == ~0) /* No solution found */
+		return -EINVAL;
+
+	return 0;
+}
+
+static int clk_pll4600c28_get_rate(unsigned long input, struct stm_pll *pll,
+			unsigned long *rate)
+{
+	if (!pll->idf)
+		pll->idf = 1;
+
+	*rate = (input / pll->idf) * 2 * pll->ndiv;
+
+	return 0;
+}
+
+static unsigned long recalc_stm_pll4600c28(struct clk_hw *hw,
+				    unsigned long parent_rate)
+{
+	struct clkgen_pll *pll = to_clkgen_pll(hw);
+	struct stm_pll params;
+	unsigned long rate;
+
+	if (!clkgen_pll_is_enabled(hw) || !clkgen_pll_is_locked(hw))
+		return 0;
+
+	params.ndiv = CLKGEN_READ(pll, ndiv);
+	params.idf = CLKGEN_READ(pll, idf);
+
+	clk_pll4600c28_get_rate(parent_rate, &params, &rate);
+
 	pr_debug("%s:%s rate %lu\n", __clk_get_name(hw->clk), __func__, rate);
 
 	return rate;
 }
 
+static long round_rate_stm_pll4600c28(struct clk_hw *hw, unsigned long rate,
+				      unsigned long *prate)
+{
+	struct stm_pll params;
+
+	if (!clk_pll4600c28_get_params(*prate, rate, &params)) {
+		clk_pll4600c28_get_rate(*prate, &params, &rate);
+	} else {
+		pr_debug("%s: %s rate %ld Invalid\n", __func__,
+			 __clk_get_name(hw->clk), rate);
+		return 0;
+	}
+
+	pr_debug("%s: %s new rate %ld [ndiv=%u] [idf=%u]\n",
+		 __func__, __clk_get_name(hw->clk),
+		 rate, (unsigned int)params.ndiv,
+		 (unsigned int)params.idf);
+
+	return rate;
+}
+
+static int set_rate_stm_pll4600c28(struct clk_hw *hw, unsigned long rate,
+				   unsigned long parent_rate)
+{
+	struct clkgen_pll *pll = to_clkgen_pll(hw);
+	struct stm_pll params;
+	long hwrate;
+	unsigned long flags = 0;
+
+	if (!rate || !parent_rate)
+		return -EINVAL;
+
+	if (!clk_pll4600c28_get_params(parent_rate, rate, &params)) {
+		clk_pll4600c28_get_rate(parent_rate, &params, &hwrate);
+	} else {
+		pr_debug("%s: %s rate %ld Invalid\n", __func__,
+			 __clk_get_name(hw->clk), rate);
+		return -EINVAL;
+	}
+
+	pr_debug("%s: %s new rate %ld [ndiv=0x%x] [idf=0x%x]\n",
+		 __func__, __clk_get_name(hw->clk),
+		 hwrate, (unsigned int)params.ndiv,
+		 (unsigned int)params.idf);
+
+	if (!hwrate)
+		return -EINVAL;
+
+	pll->ndiv = params.ndiv;
+	pll->idf = params.idf;
+
+	__clkgen_pll_disable(hw);
+
+	if (pll->lock)
+		spin_lock_irqsave(pll->lock, flags);
+
+	CLKGEN_WRITE(pll, ndiv, pll->ndiv);
+	CLKGEN_WRITE(pll, idf, pll->idf);
+
+	if (pll->lock)
+		spin_unlock_irqrestore(pll->lock, flags);
+
+	__clkgen_pll_enable(hw);
+
+	return 0;
+}
+
 static const struct clk_ops st_pll1600c65_ops = {
+	.enable		= clkgen_pll_enable,
+	.disable	= clkgen_pll_disable,
 	.is_enabled	= clkgen_pll_is_enabled,
 	.recalc_rate	= recalc_stm_pll1600c65,
 };
 
 static const struct clk_ops st_pll800c65_ops = {
+	.enable		= clkgen_pll_enable,
+	.disable	= clkgen_pll_disable,
 	.is_enabled	= clkgen_pll_is_enabled,
 	.recalc_rate	= recalc_stm_pll800c65,
 };
 
 static const struct clk_ops stm_pll3200c32_ops = {
+	.enable		= clkgen_pll_enable,
+	.disable	= clkgen_pll_disable,
 	.is_enabled	= clkgen_pll_is_enabled,
 	.recalc_rate	= recalc_stm_pll3200c32,
 };
 
+static const struct clk_ops stm_pll3200c32_a9_ops = {
+	.enable		= clkgen_pll_enable,
+	.disable	= clkgen_pll_disable,
+	.is_enabled	= clkgen_pll_is_enabled,
+	.recalc_rate	= recalc_stm_pll3200c32,
+	.round_rate	= round_rate_stm_pll3200c32,
+	.set_rate	= set_rate_stm_pll3200c32,
+};
+
 static const struct clk_ops st_pll1200c32_ops = {
+	.enable		= clkgen_pll_enable,
+	.disable	= clkgen_pll_disable,
 	.is_enabled	= clkgen_pll_is_enabled,
 	.recalc_rate	= recalc_stm_pll1200c32,
 };
 
+static const struct clk_ops stm_pll4600c28_ops = {
+	.enable		= clkgen_pll_enable,
+	.disable	= clkgen_pll_disable,
+	.is_enabled	= clkgen_pll_is_enabled,
+	.recalc_rate	= recalc_stm_pll4600c28,
+	.round_rate	= round_rate_stm_pll4600c28,
+	.set_rate	= set_rate_stm_pll4600c28,
+};
+
 static struct clk * __init clkgen_pll_register(const char *parent_name,
 				struct clkgen_pll_data	*pll_data,
 				void __iomem *reg,
-				const char *clk_name)
+				const char *clk_name, spinlock_t *lock)
 {
 	struct clkgen_pll *pll;
 	struct clk *clk;
@@ -406,13 +854,14 @@ static struct clk * __init clkgen_pll_register(const char *parent_name,
 	init.name = clk_name;
 	init.ops = pll_data->ops;
 
-	init.flags = CLK_IS_BASIC;
+	init.flags = CLK_IS_BASIC | CLK_GET_RATE_NOCACHE;
 	init.parent_names = &parent_name;
 	init.num_parents  = 1;
 
 	pll->data = pll_data;
 	pll->regs_base = reg;
 	pll->hw.init = &init;
+	pll->lock = lock;
 
 	clk = clk_register(NULL, &pll->hw);
 	if (IS_ERR(clk)) {
@@ -499,7 +948,7 @@ static void __init clkgena_c65_pll_setup(struct device_node *np)
 	 */
 	clk_data->clks[0] = clkgen_pll_register(parent_name,
 			(struct clkgen_pll_data *) &st_pll1600c65_ax,
-			reg + CLKGENAx_PLL0_OFFSET, clk_name);
+			reg + CLKGENAx_PLL0_OFFSET, clk_name, NULL);
 
 	if (IS_ERR(clk_data->clks[0]))
 		goto err;
@@ -528,7 +977,7 @@ static void __init clkgena_c65_pll_setup(struct device_node *np)
 	 */
 	clk_data->clks[2] = clkgen_pll_register(parent_name,
 			(struct clkgen_pll_data *) &st_pll800c65_ax,
-			reg + CLKGENAx_PLL1_OFFSET, clk_name);
+			reg + CLKGENAx_PLL1_OFFSET, clk_name, NULL);
 
 	if (IS_ERR(clk_data->clks[2]))
 		goto err;
@@ -544,7 +993,7 @@ CLK_OF_DECLARE(clkgena_c65_plls,
 	       "st,clkgena-plls-c65", clkgena_c65_pll_setup);
 
 static struct clk * __init clkgen_odf_register(const char *parent_name,
-					       void * __iomem reg,
+					       void __iomem *reg,
 					       struct clkgen_pll_data *pll_data,
 					       int odf,
 					       spinlock_t *odf_lock,
@@ -555,7 +1004,7 @@ static struct clk * __init clkgen_odf_register(const char *parent_name,
 	struct clk_gate *gate;
 	struct clk_divider *div;
 
-	flags = CLK_GET_RATE_NOCACHE | CLK_SET_RATE_GATE;
+	flags = CLK_GET_RATE_NOCACHE | CLK_SET_RATE_PARENT;
 
 	gate = kzalloc(sizeof(*gate), GFP_KERNEL);
 	if (!gate)
@@ -623,17 +1072,21 @@ static const struct of_device_id c32_pll_of_match[] = {
 		.data = &st_pll3200c32_407_a0,
 	},
 	{
-		.compatible = "st,stih407-plls-c32-c0_0",
-		.data = &st_pll3200c32_407_c0_0,
+		.compatible = "st,plls-c32-cx_0",
+		.data = &st_pll3200c32_cx_0,
 	},
 	{
-		.compatible = "st,stih407-plls-c32-c0_1",
-		.data = &st_pll3200c32_407_c0_1,
+		.compatible = "st,plls-c32-cx_1",
+		.data = &st_pll3200c32_cx_1,
 	},
 	{
 		.compatible = "st,stih407-plls-c32-a9",
 		.data = &st_pll3200c32_407_a9,
 	},
+	{
+		.compatible = "st,stih418-plls-c28-a9",
+		.data = &st_pll4600c28_418_a9,
+	},
 	{}
 };
 
@@ -663,7 +1116,8 @@ static void __init clkgen_c32_pll_setup(struct device_node *np)
 	if (!pll_base)
 		return;
 
-	clk = clkgen_pll_register(parent_name, data, pll_base, np->name);
+	clk = clkgen_pll_register(parent_name, data, pll_base, np->name,
+				  data->lock);
 	if (IS_ERR(clk))
 		return;
 
@@ -752,7 +1206,7 @@ static void __init clkgengpu_c32_pll_setup(struct device_node *np)
 	/*
 	 * PLL 1200MHz output
 	 */
-	clk = clkgen_pll_register(parent_name, data, reg, clk_name);
+	clk = clkgen_pll_register(parent_name, data, reg, clk_name, data->lock);
 
 	if (!IS_ERR(clk))
 		of_clk_add_provider(np, of_clk_src_simple_get, clk);
diff --git a/kernel/drivers/clk/st/clkgen.h b/kernel/drivers/clk/st/clkgen.h
index 35c863295..f7ec2d913 100644
--- a/kernel/drivers/clk/st/clkgen.h
+++ b/kernel/drivers/clk/st/clkgen.h
@@ -9,6 +9,8 @@ Copyright (C) 2014 STMicroelectronics
 #ifndef __CLKGEN_INFO_H
 #define __CLKGEN_INFO_H
 
+extern spinlock_t clkgen_a9_lock;
+
 struct clkgen_field {
 	unsigned int offset;
 	unsigned int mask;