[PATCH 32/74] ST SPEAr: Adding clk_set_rate support

@@ -87,6 +87,15 @@ static struct pll_clk_config pll1_config = {
 	.masks = &pll_masks,
 };
 
+/* pll rate configuration table, in ascending order of rates */
+struct pll_rate_tbl pll_rtbl[] = {
+	/* PCLK 24MHz */
+	{.mode = 0, .m = 0x7D, .n = 0x03, .p = 0x2}, /* 500 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x03, .p = 0x2}, /* 664 MHz */
+	{.mode = 0, .m = 0xC8, .n = 0x03, .p = 0x2}, /* 800 MHz */
+	{.mode = 0, .m = 0xFA, .n = 0x06, .p = 0x1}, /* 1000 MHz */
+};
+
 /* pll1 clock */
 static struct clk pll1_clk = {
 	.flags = ENABLED_ON_INIT,

@@ -94,7 +103,10 @@ static struct clk pll1_clk = {
 	.pclk_sel_shift = PLL1_CLK_SHIFT,
 	.en_reg = PLL1_CTR,
 	.en_reg_bit = PLL_ENABLE,
+	.calc_rate = &pll_calc_rate,
 	.recalc = &pll_clk_recalc,
+	.set_rate = &pll_clk_set_rate,
+	.rate_config = {pll_rtbl, ARRAY_SIZE(pll_rtbl), 3},
 	.private_data = &pll1_config,
 };
 

@@ -127,7 +139,10 @@ static struct clk pll2_clk = {
 	.pclk_sel_shift = PLL2_CLK_SHIFT,
 	.en_reg = PLL2_CTR,
 	.en_reg_bit = PLL_ENABLE,
+	.calc_rate = &pll_calc_rate,
 	.recalc = &pll_clk_recalc,
+	.set_rate = &pll_clk_set_rate,
+	.rate_config = {pll_rtbl, ARRAY_SIZE(pll_rtbl), 3},
 	.private_data = &pll2_config,
 };
 

@@ -144,7 +159,10 @@ static struct clk pll3_clk = {
 	.pclk_sel_shift = PLL3_CLK_SHIFT,
 	.en_reg = PLL3_CTR,
 	.en_reg_bit = PLL_ENABLE,
+	.calc_rate = &pll_calc_rate,
 	.recalc = &pll_clk_recalc,
+	.set_rate = &pll_clk_set_rate,
+	.rate_config = {pll_rtbl, ARRAY_SIZE(pll_rtbl), 3},
 	.private_data = &pll3_config,
 };
 

@@ -155,13 +173,24 @@ static struct pll_clk_config pll4_config = {
 	.masks = &pll_masks,
 };
 
+/* pll4 rate configuration table, in ascending order of rates */
+struct pll_rate_tbl pll4_rtbl[] = {
+	{.mode = 0, .m = 0x7D, .n = 0x03, .p = 0x2}, /* 500 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x03, .p = 0x2}, /* 664 MHz */
+	{.mode = 0, .m = 0xC8, .n = 0x03, .p = 0x2}, /* 800 MHz */
+	{.mode = 0, .m = 0xFA, .n = 0x06, .p = 0x1}, /* 1000 MHz */
+};
+
 /* pll4 (DDR) clock */
 static struct clk pll4_clk = {
 	.flags = ENABLED_ON_INIT,
 	.pclk = &osc1_24m_clk,
 	.en_reg = PLL4_CTR,
 	.en_reg_bit = PLL_ENABLE,
+	.calc_rate = &pll_calc_rate,
 	.recalc = &pll_clk_recalc,
+	.set_rate = &pll_clk_set_rate,
+	.rate_config = {pll4_rtbl, ARRAY_SIZE(pll4_rtbl), 3},
 	.private_data = &pll4_config,
 };
 

@@ -287,12 +316,24 @@ static struct aux_clk_config uart_synth_config = {
 	.masks = &aux_masks,
 };
 
+/* aux rate configuration table, in ascending order of rates */
+struct aux_rate_tbl aux_rtbl[] = {
+	/* For PLL1div2 = 500 MHz */
+	{.xscale = 1, .yscale = 6, .eq = 1}, /* 83 MHz */
+	{.xscale = 1, .yscale = 4, .eq = 1}, /* 125 MHz */
+	{.xscale = 1, .yscale = 3, .eq = 1}, /* 166 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* 250 MHz */
+};
+
 /* uart synth clock */
 static struct clk uart_synth_clk = {
 	.en_reg = UART_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1div2_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 0},
 	.private_data = &uart_synth_config,
 };
 

@@ -335,8 +376,11 @@ static struct clk sd_synth_clk = {
 	.en_reg = SD_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1div2_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
-	.private_data = &sd_synth_config,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 2},
+	.private_data = &sdhci_synth_config,
 };
 
 /* sd clock */

@@ -358,7 +402,10 @@ static struct clk cfxd_synth_clk = {
 	.en_reg = CFXD_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1div2_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 2},
 	.private_data = &cfxd_synth_config,
 };
 

@@ -381,7 +428,10 @@ static struct clk c3_synth_clk = {
 	.en_reg = C3_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1div2_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 0},
 	.private_data = &c3_synth_config,
 };
 

@@ -453,11 +503,23 @@ static struct clk gmac_phy_input_clk = {
 	.recalc = &follow_parent,
 };
 
+/* gmac rate configuration table, in ascending order of rates */
+struct aux_rate_tbl gmac_rtbl[] = {
+	/* For gmac phy input clk */
+	{.xscale = 1, .yscale = 6, .eq = 1}, /* divided by 6 */
+	{.xscale = 1, .yscale = 4, .eq = 1}, /* divided by 4 */
+	{.xscale = 1, .yscale = 3, .eq = 1}, /* divided by 3 */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* divided by 2 */
+};
+
 static struct clk gmac_phy_synth_clk = {
 	.en_reg = GMAC_CLK_CFG,
 	.en_reg_bit = GMAC_PHY_SYNT_ENB,
 	.pclk = &gmac_phy_input_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {gmac_rtbl, ARRAY_SIZE(gmac_rtbl), 0},
 	.private_data = &gmac_phy_synth_config,
 };
 

@@ -518,13 +580,23 @@ static struct pclk_sel clcd_synth_pclk_sel = {
 	.pclk_sel_mask = CLCD_SYNT_CLK_MASK,
 };
 
+/* clcd rate configuration table, in ascending order of rates */
+struct clcd_rate_tbl clcd_rtbl[] = {
+	/* For pll1div4 = 250 MHz */
+	{.div = 0x4000}, /* 62.5 MHz */
+	{.div = 0x2000}, /* 125 MHz */
+};
+
 /* clcd synth clock */
 static struct clk clcd_synth_clk = {
 	.en_reg = CLCD_CLK_SYNT,
 	.en_reg_bit = CLCD_SYNT_ENB,
 	.pclk_sel = &clcd_synth_pclk_sel,
 	.pclk_sel_shift = CLCD_SYNT_CLK_SHIFT,
+	.calc_rate = &clcd_calc_rate,
 	.recalc = &clcd_clk_recalc,
+	.set_rate = &clcd_clk_set_rate,
+	.rate_config = {clcd_rtbl, ARRAY_SIZE(clcd_rtbl), 1},
 	.private_data = &clcd_synth_config,
 };
 

@@ -60,13 +60,22 @@ static struct pll_clk_config pll1_config = {
 	.masks = &pll1_masks,
 };
 
+/* pll rate configuration table, in ascending order of rates */
+struct pll_rate_tbl pll_rtbl[] = {
+	{.mode = 0, .m = 0x85, .n = 0x0C, .p = 0x1}, /* 266 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x0C, .p = 0x1}, /* 332 MHz */
+};
+
 /* PLL1 clock */
 static struct clk pll1_clk = {
 	.flags = ENABLED_ON_INIT,
 	.pclk = &osc_24m_clk,
 	.en_reg = PLL1_CTR,
 	.en_reg_bit = PLL_ENABLE,
+	.calc_rate = &pll_calc_rate,
 	.recalc = &pll_clk_recalc,
+	.set_rate = &pll_clk_set_rate,
+	.rate_config = {pll_rtbl, ARRAY_SIZE(pll_rtbl), 1},
 	.private_data = &pll1_config,
 };
 

@@ -104,11 +113,22 @@ static struct bus_clk_config ahb_config = {
 	.masks = &ahb_masks,
 };
 
+/* ahb rate configuration table, in ascending order of rates */
+struct bus_rate_tbl bus_rtbl[] = {
+	{.div = 3}, /* == parent divided by 4 */
+	{.div = 2}, /* == parent divided by 3 */
+	{.div = 1}, /* == parent divided by 2 */
+	{.div = 0}, /* == parent divided by 1 */
+};
+
 /* ahb clock */
 static struct clk ahb_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &pll1_clk,
+	.calc_rate = &bus_calc_rate,
 	.recalc = &bus_clk_recalc,
+	.set_rate = &bus_clk_set_rate,
+	.rate_config = {bus_rtbl, ARRAY_SIZE(bus_rtbl), 2},
 	.private_data = &ahb_config,
 };
 

@@ -130,12 +150,23 @@ static struct aux_clk_config uart_synth_config = {
 	.masks = &aux_masks,
 };
 
+/* aux rate configuration table, in ascending order of rates */
+struct aux_rate_tbl aux_rtbl[] = {
+	/* For PLL1 = 332 MHz */
+	{.xscale = 1, .yscale = 8, .eq = 1}, /* 41.5 MHz */
+	{.xscale = 1, .yscale = 4, .eq = 1}, /* 83 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* 166 MHz */
+};
+
 /* uart synth clock */
 static struct clk uart_synth_clk = {
 	.en_reg = UART_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 1},
 	.private_data = &uart_synth_config,
 };
 

@@ -178,7 +209,10 @@ static struct clk firda_synth_clk = {
 	.en_reg = FIRDA_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 1},
 	.private_data = &firda_synth_config,
 };
 

@@ -218,6 +252,14 @@ static struct gpt_clk_masks gpt_masks = {
 	.nscale_sel_shift = GPT_NSCALE_SHIFT,
 };
 
+/* gpt rate configuration table, in ascending order of rates */
+struct gpt_rate_tbl gpt_rtbl[] = {
+	/* For pll1 = 332 MHz */
+	{.mscale = 4, .nscale = 0}, /* 41.5 MHz */
+	{.mscale = 2, .nscale = 0}, /* 55.3 MHz */
+	{.mscale = 1, .nscale = 0}, /* 83 MHz */
+};
+
 /* gpt0 synth clk config*/
 static struct gpt_clk_config gpt0_synth_config = {
 	.synth_reg = PRSC1_CLK_CFG,

@@ -228,7 +270,10 @@ static struct gpt_clk_config gpt0_synth_config = {
 static struct clk gpt0_synth_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &pll1_clk,
+	.calc_rate = &gpt_calc_rate,
 	.recalc = &gpt_clk_recalc,
+	.set_rate = &gpt_clk_set_rate,
+	.rate_config = {gpt_rtbl, ARRAY_SIZE(gpt_rtbl), 2},
 	.private_data = &gpt0_synth_config,
 };
 

@@ -269,7 +314,10 @@ static struct gpt_clk_config gpt1_synth_config = {
 static struct clk gpt1_synth_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &pll1_clk,
+	.calc_rate = &gpt_calc_rate,
 	.recalc = &gpt_clk_recalc,
+	.set_rate = &gpt_clk_set_rate,
+	.rate_config = {gpt_rtbl, ARRAY_SIZE(gpt_rtbl), 2},
 	.private_data = &gpt1_synth_config,
 };
 

@@ -310,7 +358,10 @@ static struct gpt_clk_config gpt2_synth_config = {
 static struct clk gpt2_synth_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &pll1_clk,
+	.calc_rate = &gpt_calc_rate,
 	.recalc = &gpt_clk_recalc,
+	.set_rate = &gpt_clk_set_rate,
+	.rate_config = {gpt_rtbl, ARRAY_SIZE(gpt_rtbl), 2},
 	.private_data = &gpt2_synth_config,
 };
 

@@ -382,7 +433,10 @@ static struct bus_clk_config apb_config = {
 static struct clk apb_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &ahb_clk,
+	.calc_rate = &bus_calc_rate,
 	.recalc = &bus_clk_recalc,
+	.set_rate = &bus_clk_set_rate,
+	.rate_config = {bus_rtbl, ARRAY_SIZE(bus_rtbl), 2},
 	.private_data = &apb_config,
 };
 

@@ -60,13 +60,22 @@ static struct pll_clk_config pll1_config = {
 	.masks = &pll1_masks,
 };
 
+/* pll rate configuration table, in ascending order of rates */
+struct pll_rate_tbl pll_rtbl[] = {
+	{.mode = 0, .m = 0x85, .n = 0x0C, .p = 0x1}, /* 266 MHz */
+	{.mode = 0, .m = 0xA6, .n = 0x0C, .p = 0x1}, /* 332 MHz */
+};
+
 /* PLL1 clock */
 static struct clk pll1_clk = {
 	.flags = ENABLED_ON_INIT,
 	.pclk = &osc_30m_clk,
 	.en_reg = PLL1_CTR,
 	.en_reg_bit = PLL_ENABLE,
+	.calc_rate = &pll_calc_rate,
 	.recalc = &pll_clk_recalc,
+	.set_rate = &pll_clk_set_rate,
+	.rate_config = {pll_rtbl, ARRAY_SIZE(pll_rtbl), 1},
 	.private_data = &pll1_config,
 };
 

@@ -104,11 +113,22 @@ static struct bus_clk_config ahb_config = {
 	.masks = &ahb_masks,
 };
 
+/* ahb rate configuration table, in ascending order of rates */
+struct bus_rate_tbl bus_rtbl[] = {
+	{.div = 3}, /* == parent divided by 4 */
+	{.div = 2}, /* == parent divided by 3 */
+	{.div = 1}, /* == parent divided by 2 */
+	{.div = 0}, /* == parent divided by 1 */
+};
+
 /* ahb clock */
 static struct clk ahb_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &pll1_clk,
+	.calc_rate = &bus_calc_rate,
 	.recalc = &bus_clk_recalc,
+	.set_rate = &bus_clk_set_rate,
+	.rate_config = {bus_rtbl, ARRAY_SIZE(bus_rtbl), 2},
 	.private_data = &ahb_config,
 };
 

@@ -130,12 +150,23 @@ static struct aux_clk_config uart_synth_config = {
 	.masks = &aux_masks,
 };
 
+/* aux rate configuration table, in ascending order of rates */
+struct aux_rate_tbl aux_rtbl[] = {
+	/* For PLL1 = 332 MHz */
+	{.xscale = 1, .yscale = 8, .eq = 1}, /* 41.5 MHz */
+	{.xscale = 1, .yscale = 4, .eq = 1}, /* 83 MHz */
+	{.xscale = 1, .yscale = 2, .eq = 1}, /* 166 MHz */
+};
+
 /* uart synth clock */
 static struct clk uart_synth_clk = {
 	.en_reg = UART_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 2},
 	.private_data = &uart_synth_config,
 };
 

@@ -187,7 +218,10 @@ static struct clk firda_synth_clk = {
 	.en_reg = FIRDA_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 2},
 	.private_data = &firda_synth_config,
 };
 

@@ -230,7 +264,10 @@ static struct clk clcd_synth_clk = {
 	.en_reg = CLCD_CLK_SYNT,
 	.en_reg_bit = AUX_SYNT_ENB,
 	.pclk = &pll1_clk,
+	.calc_rate = &aux_calc_rate,
 	.recalc = &aux_clk_recalc,
+	.set_rate = &aux_clk_set_rate,
+	.rate_config = {aux_rtbl, ARRAY_SIZE(aux_rtbl), 2},
 	.private_data = &clcd_synth_config,
 };
 

@@ -270,6 +307,14 @@ static struct gpt_clk_masks gpt_masks = {
 	.nscale_sel_shift = GPT_NSCALE_SHIFT,
 };
 
+/* gpt rate configuration table, in ascending order of rates */
+struct gpt_rate_tbl gpt_rtbl[] = {
+	/* For pll1 = 332 MHz */
+	{.mscale = 4, .nscale = 0}, /* 41.5 MHz */
+	{.mscale = 2, .nscale = 0}, /* 55.3 MHz */
+	{.mscale = 1, .nscale = 0}, /* 83 MHz */
+};
+
 /* gpt0 synth clk config*/
 static struct gpt_clk_config gpt0_synth_config = {
 	.synth_reg = PRSC1_CLK_CFG,

@@ -280,7 +325,10 @@ static struct gpt_clk_config gpt0_synth_config = {
 static struct clk gpt0_synth_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &pll1_clk,
+	.calc_rate = &gpt_calc_rate,
 	.recalc = &gpt_clk_recalc,
+	.set_rate = &gpt_clk_set_rate,
+	.rate_config = {gpt_rtbl, ARRAY_SIZE(gpt_rtbl), 2},
 	.private_data = &gpt0_synth_config,
 };
 

@@ -339,7 +387,10 @@ static struct gpt_clk_config gpt2_synth_config = {
 static struct clk gpt2_synth_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &pll1_clk,
+	.calc_rate = &gpt_calc_rate,
 	.recalc = &gpt_clk_recalc,
+	.set_rate = &gpt_clk_set_rate,
+	.rate_config = {gpt_rtbl, ARRAY_SIZE(gpt_rtbl), 2},
 	.private_data = &gpt2_synth_config,
 };
 

@@ -380,7 +431,10 @@ static struct gpt_clk_config gpt3_synth_config = {
 static struct clk gpt3_synth_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &pll1_clk,
+	.calc_rate = &gpt_calc_rate,
 	.recalc = &gpt_clk_recalc,
+	.set_rate = &gpt_clk_set_rate,
+	.rate_config = {gpt_rtbl, ARRAY_SIZE(gpt_rtbl), 2},
 	.private_data = &gpt3_synth_config,
 };
 

@@ -453,7 +507,10 @@ static struct bus_clk_config apb_config = {
 static struct clk apb_clk = {
 	.flags = ALWAYS_ENABLED,
 	.pclk = &ahb_clk,
+	.calc_rate = &bus_calc_rate,
 	.recalc = &bus_clk_recalc,
+	.set_rate = &bus_clk_set_rate,
+	.rate_config = {bus_rtbl, ARRAY_SIZE(bus_rtbl), 2},
 	.private_data = &apb_config,
 };
 

@@ -580,6 +637,7 @@ static struct clk_lookup spear_clk_lookups[] = {
 	{ .con_id = "ahb_clk",		.clk = &ahb_clk},
 	{ .con_id = "uart_synth_clk",	.clk = &uart_synth_clk},
 	{ .con_id = "firda_synth_clk",	.clk = &firda_synth_clk},
+	{ .con_id = "clcd_synth_clk",	.clk = &clcd_synth_clk},
 	{ .con_id = "gpt0_synth_clk",	.clk = &gpt0_synth_clk},
 	{ .con_id = "gpt2_synth_clk",	.clk = &gpt2_synth_clk},
 	{ .con_id = "gpt3_synth_clk",	.clk = &gpt3_synth_clk},

@@ -27,28 +27,11 @@ static LIST_HEAD(root_clks);
 static LIST_HEAD(clocks);
 #endif
 
-static void propagate_rate(struct clk *);
+static void propagate_rate(struct clk *, int on_init);
 #ifdef CONFIG_DEBUG_FS
 static int clk_debugfs_reparent(struct clk *);
 #endif
 
-static int clk_is_enabled(struct clk *clk)
-{
-	unsigned int val;
-
-	if (clk->flags & ALWAYS_ENABLED)
-		return 1;
-
-	BUG_ON(!clk->en_reg);
-	val = readl(clk->en_reg);
-	val = val & (1 << clk->en_reg_bit);
-
-	if (unlikely(clk->flags & RESET_TO_ENABLE))
-		return !val;
-	else
-		return !!val;
-}
-
 static int generic_clk_enable(struct clk *clk)
 {
 	unsigned int val;

@@ -126,7 +109,7 @@ static void clk_reparent(struct clk *clk, struct pclk_info *pclk_info)
 #endif
 }
 
-void do_clk_disable(struct clk *clk)
+static void do_clk_disable(struct clk *clk)
 {
 	if (!clk)
 		return;

@@ -151,7 +134,7 @@ void do_clk_disable(struct clk *clk)
 	}
 }
 
-int do_clk_enable(struct clk *clk)
+static int do_clk_enable(struct clk *clk)
 {
 	int ret = 0;
 

@@ -176,8 +159,11 @@ int do_clk_enable(struct clk *clk)
 		 * Since the clock is going to be used for the first
 		 * time please reclac
 		 */
-		if (clk->recalc)
-			clk->recalc(clk);
+		if (clk->recalc) {
+			ret = clk->recalc(clk);
+			if (ret)
+				goto err;
+		}
 	}
 	clk->usage_count++;
 err:

@@ -248,8 +234,7 @@ EXPORT_SYMBOL(clk_get_rate);
  * @clk: clock source
  * @parent: parent clock source
  *
- * Returns success (0) or negative errno. clk usage_count must be zero
- * before calling this function.
+ * Returns success (0) or negative errno.
  */
 int clk_set_parent(struct clk *clk, struct clk *parent)
 {

@@ -258,8 +243,6 @@ int clk_set_parent(struct clk *clk, struct clk *parent)
 
 	if (!clk || !parent)
 		return -EFAULT;
-	if (clk->usage_count)
-		return -EPERM;
 	if (clk->pclk == parent)
 		return 0;
 	if (!clk->pclk_sel)

@@ -287,6 +270,7 @@ int clk_set_parent(struct clk *clk, struct clk *parent)
 	/* reflect parent change in software */
 	clk_reparent(clk, &clk->pclk_sel->pclk_info[i]);
 
+	propagate_rate(clk, 0);
 	return 0;
 }
 EXPORT_SYMBOL(clk_set_parent);

@@ -300,8 +284,25 @@ EXPORT_SYMBOL(clk_set_parent);
  */
 int clk_set_rate(struct clk *clk, unsigned long rate)
 {
-	/* TODO */
-	return -EINVAL;
+	unsigned long flags;
+	int ret = -EINVAL;
+
+	if (!clk || !rate)
+		return -EFAULT;
+
+	if (clk->set_rate) {
+		spin_lock_irqsave(&clocks_lock, flags);
+		ret = clk->set_rate(clk, rate);
+		if (!ret)
+			/* if successful -> propagate */
+			propagate_rate(clk, 0);
+		spin_unlock_irqrestore(&clocks_lock, flags);
+	} else if (clk->pclk) {
+		u32 mult = clk->div_factor ? clk->div_factor : 1;
+		ret = clk_set_rate(clk->pclk, mult * rate);
+	}
+
+	return ret;
 }
 EXPORT_SYMBOL(clk_set_rate);
 

@@ -358,27 +359,112 @@ void clk_register(struct clk_lookup *cl)
 
 /**
  * propagate_rate - recalculate and propagate all clocks to children
+ * @pclk: parent clock required to be propogated
+ * @on_init: flag for enabling clocks which are ENABLED_ON_INIT.
  *
  * Recalculates all children clocks
  */
-void propagate_rate(struct clk *pclk)
+void propagate_rate(struct clk *pclk, int on_init)
 {
 	struct clk *clk, *_temp;
+	int ret = 0;
 
 	list_for_each_entry_safe(clk, _temp, &pclk->children, sibling) {
-		/* recalc and propogate only if clk is enabled */
-		if (clk_is_enabled(clk)) {
-			if (clk->recalc)
-				clk->recalc(clk);
-			propagate_rate(clk);
+		if (clk->recalc) {
+			ret = clk->recalc(clk);
+			/*
+			 * recalc will return error if clk out is not programmed
+			 * In this case configure default rate.
+			 */
+			if (ret && clk->set_rate)
+				clk->set_rate(clk, 0);
 		}
+		propagate_rate(clk, on_init);
+
+		if (!on_init)
+			continue;
+
 		/* Enable clks enabled on init, in software view */
 		if (clk->flags & ENABLED_ON_INIT)
 			do_clk_enable(clk);
 	}
 }
 
-/*All recalc functions are called with lock held */
+/**
+ * round_rate - Returns index of closest programmable rate in rate_config tbl
+ * @clk: ptr to clock structure
+ * @drate: desired rate
+ * @rate: final rate will be returned in this variable only.
+ *
+ * Finds index in rate_config for highest clk rate which is less than
+ * requested rate. If there is no clk rate lesser than requested rate then
+ * -EINVAL is returned. This routine assumes that rate_config is written
+ * in incrementing order of clk rates.
+ * If drate passed is zero then default rate is programmed.
+ */
+static int round_rate(struct clk *clk, unsigned long drate, unsigned long *rate)
+{
+	unsigned long tmp = 0, prev_rate = 0;
+	int index;
+
+	if (!clk->calc_rate)
+		return -EFAULT;
+
+	/* Set default rate if desired rate is 0 */
+	if (!drate) {
+		index = clk->rate_config.default_index;
+		*rate = clk->calc_rate(clk, index);
+		return index;
+	}
+
+	/*
+	 * This loops ends on two conditions:
+	 * - as soon as clk is found with rate greater than requested rate.
+	 * - if all clks in rate_config are smaller than requested rate.
+	 */
+	for (index = 0; index < clk->rate_config.count; index++) {
+		prev_rate = tmp;
+		tmp = clk->calc_rate(clk, index);
+		if (drate < tmp) {
+			index--;
+			break;
+		}
+	}
+	/* return if can't find suitable clock */
+	if (index < 0) {
+		index = -EINVAL;
+		*rate = 0;
+	} else if (index == clk->rate_config.count) {
+		/* program with highest clk rate possible */
+		index = clk->rate_config.count - 1;
+		*rate = tmp;
+	} else
+		*rate = prev_rate;
+
+	return index;
+}
+
+/*All below functions are called with lock held */
+
+/*
+ * Calculates pll clk rate for specific value of mode, m, n and p
+ *
+ * In normal mode
+ * rate = (2 * M[15:8] * Fin)/(N * 2^P)
+ *
+ * In Dithered mode
+ * rate = (2 * M[15:0] * Fin)/(256 * N * 2^P)
+ */
+unsigned long pll_calc_rate(struct clk *clk, int index)
+{
+	unsigned long rate = clk->pclk->rate;
+	struct pll_rate_tbl *tbls = clk->rate_config.tbls;
+	unsigned int mode;
+
+	mode = tbls[index].mode ? 256 : 1;
+	return (((2 * rate / 10000) * tbls[index].m) /
+			(mode * tbls[index].n * (1 << tbls[index].p))) * 10000;
+}
 
 /*
  * calculates current programmed rate of pll1

@@ -389,7 +475,7 @@ void propagate_rate(struct clk *pclk)
  * In Dithered mode
  * rate = (2 * M[15:0] * Fin)/(256 * N * 2^P)
  */
-void pll_clk_recalc(struct clk *clk)
+int pll_clk_recalc(struct clk *clk)
 {
 	struct pll_clk_config *config = clk->private_data;
 	unsigned int num = 2, den = 0, val, mode = 0;

@@ -415,13 +501,69 @@ void pll_clk_recalc(struct clk *clk)
 		den *= 256;
 	}
 
-	BUG_ON(!den);
+	if (!den)
+		return -EINVAL;
 
 	clk->rate = (((clk->pclk->rate/10000) * num) / den) * 10000;
+	return 0;
+}
+
+/*
+ * Configures new clock rate of pll
+ */
+int pll_clk_set_rate(struct clk *clk, unsigned long desired_rate)
+{
+	struct pll_rate_tbl *tbls = clk->rate_config.tbls;
+	struct pll_clk_config *config = clk->private_data;
+	unsigned long val, rate;
+	int i;
+
+	i = round_rate(clk, desired_rate, &rate);
+	if (i < 0)
+		return i;
+
+	val = readl(config->mode_reg) &
+		~(config->masks->mode_mask << config->masks->mode_shift);
+	val |= (tbls[i].mode & config->masks->mode_mask) <<
+		config->masks->mode_shift;
+	writel(val, config->mode_reg);
+
+	val = readl(config->cfg_reg) &
+		~(config->masks->div_p_mask << config->masks->div_p_shift);
+	val |= (tbls[i].p & config->masks->div_p_mask) <<
+		config->masks->div_p_shift;
+	val &= ~(config->masks->div_n_mask << config->masks->div_n_shift);
+	val |= (tbls[i].n & config->masks->div_n_mask) <<
+		config->masks->div_n_shift;
+	val &= ~(config->masks->dith_fdbk_m_mask <<
+			config->masks->dith_fdbk_m_shift);
+	if (tbls[i].mode)
+		val |= (tbls[i].m & config->masks->dith_fdbk_m_mask) <<
+			config->masks->dith_fdbk_m_shift;
+	else
+		val |= (tbls[i].m & config->masks->norm_fdbk_m_mask) <<
+			config->masks->norm_fdbk_m_shift;
+
+	writel(val, config->cfg_reg);
+
+	clk->rate = rate;
+
+	return 0;
+}
+
+/*
+ * Calculates ahb, apb clk rate for specific value of div
+ */
+unsigned long bus_calc_rate(struct clk *clk, int index)
+{
+	unsigned long rate = clk->pclk->rate;
+	struct bus_rate_tbl *tbls = clk->rate_config.tbls;
+
+	return rate / (tbls[index].div + 1);
 }
 
 /* calculates current programmed rate of ahb or apb bus */
-void bus_clk_recalc(struct clk *clk)
+int bus_clk_recalc(struct clk *clk)
 {
 	struct bus_clk_config *config = clk->private_data;
 	unsigned int div;