clk: mdss: add software mux for byte and pixel source clocks

Implement the software mux to byte clock and pixel source clock
with shadow implementation of PLL clock. This is used for
configuring the dynamic refresh pll registers.

Change-Id: I9c84cb76d040c5df7361291b6e1fc0fe69dc214f
Signed-off-by: Jeevan Shriram <jshriram@codeaurora.org>

4 files changed, 612 insertions, 55 deletions

diff --git a/drivers/clk/msm/mdss/mdss-dsi-20nm-pll-util.c b/drivers/clk/msm/mdss/mdss-dsi-20nm-pll-util.c
index 9ee5748a2372..316db499fcf2 100644
--- a/drivers/clk/msm/mdss/mdss-dsi-20nm-pll-util.c
+++ b/drivers/clk/msm/mdss/mdss-dsi-20nm-pll-util.c
@@ -98,10 +98,118 @@
 #define MMSS_DSI_PHY_PLL_HR_OCLK3_DIVIDER		0x0148
 #define MMSS_DSI_PHY_PLL_PLL_VCO_HIGH			0x014C
 #define MMSS_DSI_PHY_PLL_RESET_SM			0x0150
+#define MMSS_DSI_PHY_PLL_MUXVAL			0x0154
+#define MMSS_DSI_PHY_PLL_CORE_RES_CODE_DN		0x0158
+#define MMSS_DSI_PHY_PLL_CORE_RES_CODE_UP		0x015C
+#define MMSS_DSI_PHY_PLL_CORE_VCO_TUNE			0x0160
+#define MMSS_DSI_PHY_PLL_CORE_VCO_TAIL			0x0164
+#define MMSS_DSI_PHY_PLL_CORE_KVCO_CODE		0x0168
+
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL0		0x014
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL1		0x018
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL2		0x01C
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL3		0x020
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL4		0x024
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL5		0x028
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL6		0x02C
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL7		0x030
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL8		0x034
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL9		0x038
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL10		0x03C
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL11		0x040
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL12		0x044
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL13		0x048
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL14		0x04C
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL15		0x050
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL16		0x054
+#define MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL17		0x058
 
 #define DSI_PLL_POLL_MAX_READS			15
 #define DSI_PLL_POLL_TIMEOUT_US			1000
 
+int set_mdss_byte_mux_sel(struct mux_clk *clk, int sel)
+{
+	return 0;
+}
+
+int get_mdss_byte_mux_sel(struct mux_clk *clk)
+{
+	return 0;
+}
+
+int set_mdss_pixel_mux_sel(struct mux_clk *clk, int sel)
+{
+	return 0;
+}
+
+int get_mdss_pixel_mux_sel(struct mux_clk *clk)
+{
+	return 0;
+}
+
+void dsi_cache_trim_codes(struct mdss_pll_resources *dsi_pll_res)
+{
+	int rc;
+
+	if (dsi_pll_res->reg_upd)
+		return;
+
+	rc = mdss_pll_resource_enable(dsi_pll_res, true);
+	if (rc) {
+		pr_err("Failed to enable mdss dsi pll resources\n");
+		return;
+	}
+
+	dsi_pll_res->cache_pll_trim_codes[0] =
+		(MDSS_PLL_REG_R(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_CORE_KVCO_CODE) & ~0xF);
+	dsi_pll_res->cache_pll_trim_codes[1] =
+		(MDSS_PLL_REG_R(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_CORE_VCO_TAIL) & ~0xF);
+	dsi_pll_res->cache_pll_trim_codes[2] =
+		(MDSS_PLL_REG_R(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_CORE_VCO_TUNE) & ~0x7F);
+	dsi_pll_res->cache_pll_trim_codes[3] =
+		(MDSS_PLL_REG_R(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_CORE_RES_CODE_UP) & ~0xF);
+	dsi_pll_res->cache_pll_trim_codes[4] =
+		(MDSS_PLL_REG_R(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_CORE_RES_CODE_DN) & ~0xF);
+
+	mdss_pll_resource_enable(dsi_pll_res, false);
+
+	dsi_pll_res->reg_upd = true;
+}
+
+static void dsi_dfps_override_trim_codes(struct mdss_pll_resources *dsi_pll_res)
+{
+	int reg_data;
+
+	/*
+	 * Override mux config for all cached trim codes from
+	 * saved config except for VCO Tune
+	 */
+	reg_data = dsi_pll_res->cache_pll_trim_codes[0];
+	reg_data |= BIT(5);
+	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_KVCO_CODE, reg_data);
+
+	reg_data = dsi_pll_res->cache_pll_trim_codes[1] << 2;
+	reg_data |= BIT(7);
+	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_PLL_VCOTAIL_EN, reg_data);
+
+	reg_data = dsi_pll_res->cache_pll_trim_codes[3];
+	reg_data |= BIT(5);
+	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_RES_CODE_UP, reg_data);
+
+	reg_data = dsi_pll_res->cache_pll_trim_codes[4];
+	reg_data |= BIT(5);
+	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
+		MMSS_DSI_PHY_PLL_RES_CODE_DN, reg_data);
+
+}
 
 int set_bypass_lp_div_mux_sel(struct mux_clk *clk, int sel)
 {
@@ -121,6 +229,33 @@ int set_bypass_lp_div_mux_sel(struct mux_clk *clk, int sel)
 	return 0;
 }
 
+int set_shadow_bypass_lp_div_mux_sel(struct mux_clk *clk, int sel)
+{
+	struct mdss_pll_resources *dsi_pll_res = clk->priv;
+	int reg_data, rem;
+
+	if (!dsi_pll_res->resource_enable) {
+		pr_err("PLL resources disabled. Dynamic fps invalid\n");
+		return -EINVAL;
+	}
+
+	reg_data = MDSS_PLL_REG_R(dsi_pll_res->pll_base,
+			MMSS_DSI_PHY_PLL_POST_DIVIDER_CONTROL);
+	reg_data |= BIT(7);
+
+	pr_debug("%d: reg_data = %x\n", __LINE__, reg_data);
+
+	/* Repeat POST DIVIDER 2 times (4 writes)*/
+	for (rem = 0; rem < 2; rem++)
+		MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+			MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL16 + (4 * rem),
+			MMSS_DSI_PHY_PLL_POST_DIVIDER_CONTROL,
+			MMSS_DSI_PHY_PLL_POST_DIVIDER_CONTROL,
+			(reg_data | (sel << 5)), (reg_data | (sel << 5)));
+
+	return 0;
+}
+
 int get_bypass_lp_div_mux_sel(struct mux_clk *clk)
 {
 	int mux_mode, rc;
@@ -147,6 +282,8 @@ int ndiv_set_div(struct div_clk *clk, int div)
 	int rc, reg_data;
 	struct mdss_pll_resources *dsi_pll_res = clk->priv;
 
+	pr_debug("%d div=%i\n", __LINE__, div);
+
 	rc = mdss_pll_resource_enable(dsi_pll_res, true);
 	if (rc) {
 		pr_err("Failed to enable mdss dsi pll resources\n");
@@ -163,6 +300,26 @@ int ndiv_set_div(struct div_clk *clk, int div)
 	return rc;
 }
 
+int shadow_ndiv_set_div(struct div_clk *clk, int div)
+{
+	struct mdss_pll_resources *dsi_pll_res = clk->priv;
+
+	if (!dsi_pll_res->resource_enable) {
+		pr_err("PLL resources disabled. Dynamic fps invalid\n");
+		return -EINVAL;
+	}
+
+	pr_debug("%d div=%i\n", __LINE__, div);
+
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL14,
+		MMSS_DSI_PHY_PLL_RESETSM_CNTRL3,
+		MMSS_DSI_PHY_PLL_POST_DIVIDER_CONTROL,
+		0x07, (0xB | div));
+
+	return 0;
+}
+
 int ndiv_get_div(struct div_clk *clk)
 {
 	int div = 0, rc;
@@ -201,6 +358,25 @@ int fixed_hr_oclk2_set_div(struct div_clk *clk, int div)
 	return rc;
 }
 
+int shadow_fixed_hr_oclk2_set_div(struct div_clk *clk, int div)
+{
+	struct mdss_pll_resources *dsi_pll_res = clk->priv;
+
+	if (!dsi_pll_res->resource_enable) {
+		pr_err("PLL resources disabled. Dynamic fps invalid\n");
+		return -EINVAL;
+	}
+	pr_debug("%d div = %d\n", __LINE__, div);
+
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL5,
+		MMSS_DSI_PHY_PLL_HR_OCLK2_DIVIDER,
+		MMSS_DSI_PHY_PLL_HR_OCLK2_DIVIDER,
+		(div - 1), (div - 1));
+
+	return 0;
+}
+
 int fixed_hr_oclk2_get_div(struct div_clk *clk)
 {
 	int div = 0, rc;
@@ -238,6 +414,26 @@ int hr_oclk3_set_div(struct div_clk *clk, int div)
 	return rc;
 }
 
+int shadow_hr_oclk3_set_div(struct div_clk *clk, int div)
+{
+	struct mdss_pll_resources *dsi_pll_res = clk->priv;
+
+	if (!dsi_pll_res->resource_enable) {
+		pr_err("PLL resources disabled. Dynamic fps invalid\n");
+		return -EINVAL;
+	}
+
+	pr_debug("%d div = %d\n", __LINE__, div);
+
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL6,
+		MMSS_DSI_PHY_PLL_HR_OCLK3_DIVIDER,
+		MMSS_DSI_PHY_PLL_HR_OCLK3_DIVIDER,
+		(div - 1), (div - 1));
+
+	return 0;
+}
+
 int hr_oclk3_get_div(struct div_clk *clk)
 {
 	int div = 0, rc;
@@ -478,93 +674,103 @@ static void pll_20nm_phy_config(struct dsi_pll_vco_clk *vco)
 	pll_20nm_phy_loop_bw_config(dsi_pll_res);
 }
 
-int pll_20nm_vco_set_rate(struct dsi_pll_vco_clk *vco, unsigned long rate)
+static void pll_20nm_vco_rate_calc(struct dsi_pll_vco_clk *vco,
+		struct mdss_pll_vco_calc *vco_calc, s64 vco_clk_rate)
 {
-	s64 vco_clk_rate = rate;
+	s64 multiplier = (1 << 20);
+	s64 duration = 128, pll_comp_val;
+	s64 dec_start_multiple, dec_start;
 	s32 div_frac_start;
-	s32 div_frac_start1, div_frac_start2, div_frac_start3;
-	s64 dec_start_multiple, dec_start, multiplier = (1 << 20);
 	s64 dec_start1, dec_start2;
-	s64 duration = 128, pll_comp_val;
+	s32 div_frac_start1, div_frac_start2, div_frac_start3;
 	s64 pll_plllock_cmp1, pll_plllock_cmp2, pll_plllock_cmp3;
 
-	struct mdss_pll_resources *dsi_pll_res = vco->priv;
-
-	pr_debug("%s: vco set rate: %lld\n", __func__, vco_clk_rate);
-	pll_20nm_phy_config(vco);
-
+	memset(vco_calc, 0, sizeof(*vco_calc));
 	dec_start_multiple = div_s64(vco_clk_rate * multiplier,
 					2 * vco->ref_clk_rate);
 	div_s64_rem(dec_start_multiple,
 			multiplier, &div_frac_start);
 
 	dec_start = div_s64(dec_start_multiple, multiplier);
-	pr_debug("%s: dec_start_multiple = 0x%llx\n",
-				__func__, dec_start_multiple);
-	pr_debug("%s: dec_start = 0x%llx, div_frac_start = 0x%x\n",
-			__func__, dec_start, div_frac_start);
-
 	dec_start1 = (dec_start & 0x7f) | BIT(7);
 	dec_start2 = ((dec_start & 0x80) >> 7) | BIT(1);
-	pr_debug("%s: dec_start1 = 0x%llx, dec_start2 = 0x%llx\n",
-				__func__, dec_start1, dec_start2);
-
 	div_frac_start1 = (div_frac_start & 0x7f) | BIT(7);
 	div_frac_start2 = ((div_frac_start >> 7) & 0x7f) | BIT(7);
 	div_frac_start3 = ((div_frac_start >> 14) & 0x3f) | BIT(6);
-	pr_debug("%s: div_frac_start1 = 0x%x\n",
-				__func__, div_frac_start1);
-	pr_debug("%s: div_frac_start2 = 0x%x\n",
-				__func__, div_frac_start2);
-	pr_debug("%s: div_frac_start3 = 0x%x\n",
-				__func__, div_frac_start3);
+	pll_comp_val = div_s64(dec_start_multiple * 2 * (duration - 1),
+				10 * multiplier);
+	pll_plllock_cmp1 = pll_comp_val & 0xff;
+	pll_plllock_cmp2 = (pll_comp_val >> 8) & 0xff;
+	pll_plllock_cmp3 = (pll_comp_val >> 16) & 0xff;
+
+	pr_debug("dec_start_multiple = 0x%llx\n", dec_start_multiple);
+	pr_debug("dec_start = 0x%llx, div_frac_start = 0x%x\n",
+			dec_start, div_frac_start);
+	pr_debug("dec_start1 = 0x%llx, dec_start2 = 0x%llx\n",
+			dec_start1, dec_start2);
+	pr_debug("div_frac_start1 = 0x%x, div_frac_start2 = 0x%x\n",
+			div_frac_start1, div_frac_start2);
+	pr_debug("div_frac_start3 = 0x%x\n", div_frac_start3);
+	pr_debug("pll_comp_val = 0x%llx\n", pll_comp_val);
+	pr_debug("pll_plllock_cmp1 = 0x%llx, pll_plllock_cmp2 =%llx\n",
+			pll_plllock_cmp1, pll_plllock_cmp2);
+	pr_debug("pll_plllock_cmp3 = 0x%llx\n",	pll_plllock_cmp3);
+
+	/* Assign to vco struct */
+	vco_calc->div_frac_start1 = div_frac_start1;
+	vco_calc->div_frac_start2 = div_frac_start2;
+	vco_calc->div_frac_start3 = div_frac_start3;
+	vco_calc->dec_start1 = dec_start1;
+	vco_calc->dec_start2 = dec_start2;
+	vco_calc->pll_plllock_cmp1 = pll_plllock_cmp1;
+	vco_calc->pll_plllock_cmp2 = pll_plllock_cmp2;
+	vco_calc->pll_plllock_cmp3 = pll_plllock_cmp3;
+}
+
+int pll_20nm_vco_set_rate(struct dsi_pll_vco_clk *vco, unsigned long rate)
+{
+	s64 vco_clk_rate = rate;
+	struct mdss_pll_resources *dsi_pll_res = vco->priv;
+	struct mdss_pll_vco_calc vco_calc;
 
+	pr_debug("vco set rate: %lld\n", vco_clk_rate);
+	pll_20nm_phy_config(vco);
+
+	/* div fraction, start and comp calculations */
+	pll_20nm_vco_rate_calc(vco, &vco_calc, vco_clk_rate);
+
+	/* register programming*/
 	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_DIV_FRAC_START1,
-			div_frac_start1);
+			vco_calc.div_frac_start1);
 
 	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_DIV_FRAC_START2,
-			div_frac_start2);
+			vco_calc.div_frac_start2);
 
 	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_DIV_FRAC_START3,
-			div_frac_start3);
+			vco_calc.div_frac_start3);
 
 	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_DEC_START1,
-			dec_start1);
+			vco_calc.dec_start1);
 
 	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_DEC_START2,
-			dec_start2);
-
-	pll_comp_val = div_s64(dec_start_multiple * 2 * (duration - 1),
-				10 * multiplier);
-
-	pr_debug("%s: pll_comp_val = 0x%llx\n", __func__, pll_comp_val);
-
-	pll_plllock_cmp1 = pll_comp_val & 0xff;
-	pll_plllock_cmp2 = (pll_comp_val >> 8) & 0xff;
-	pll_plllock_cmp3 = (pll_comp_val >> 16) & 0xff;
-	pr_debug("%s: pll_plllock_cmp1 = 0x%llx\n",
-				__func__, pll_plllock_cmp1);
-	pr_debug("%s: pll_plllock_cmp2 = 0x%llx\n",
-				__func__, pll_plllock_cmp2);
-	pr_debug("%s: pll_plllock_cmp3 = 0x%llx\n",
-				__func__, pll_plllock_cmp3);
+			vco_calc.dec_start2);
 
 	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_PLLLOCK_CMP1,
-			pll_plllock_cmp1);
+			vco_calc.pll_plllock_cmp1);
 
 	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_PLLLOCK_CMP2,
-			pll_plllock_cmp2);
+			vco_calc.pll_plllock_cmp2);
 
 	MDSS_PLL_REG_W(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_PLLLOCK_CMP3,
-			pll_plllock_cmp3);
+			vco_calc.pll_plllock_cmp3);
 
 	/*
 	 * Make sure that PLL vco configuration is complete
@@ -575,6 +781,86 @@ int pll_20nm_vco_set_rate(struct dsi_pll_vco_clk *vco, unsigned long rate)
 	return 0;
 }
 
+int shadow_pll_20nm_vco_set_rate(struct dsi_pll_vco_clk *vco,
+		unsigned long rate)
+{
+	struct mdss_pll_resources *dsi_pll_res = vco->priv;
+	struct mdss_pll_vco_calc vco_calc;
+	s64 vco_clk_rate = rate;
+	u32 rem;
+
+	if (!dsi_pll_res->resource_enable) {
+		pr_err("PLL resources disabled. Dynamic fps invalid\n");
+		return -EINVAL;
+	}
+
+	pr_debug("req vco set rate: %lld\n", vco_clk_rate);
+
+	dsi_dfps_override_trim_codes(dsi_pll_res);
+	/* div fraction, start and comp calculations */
+	pll_20nm_vco_rate_calc(vco, &vco_calc, vco_clk_rate);
+
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL0,
+		MMSS_DSI_PHY_PLL_POST_DIVIDER_CONTROL,
+		MMSS_DSI_PHY_PLL_PLLLOCK_CMP_EN,
+		0xB1, 0);
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL1,
+		MMSS_DSI_PHY_PLL_PLLLOCK_CMP1,
+		MMSS_DSI_PHY_PLL_PLLLOCK_CMP2,
+		vco_calc.pll_plllock_cmp1, vco_calc.pll_plllock_cmp2);
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL2,
+		MMSS_DSI_PHY_PLL_PLLLOCK_CMP3,
+		MMSS_DSI_PHY_PLL_DEC_START1,
+		vco_calc.pll_plllock_cmp3, vco_calc.dec_start1);
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL3,
+		MMSS_DSI_PHY_PLL_DEC_START2,
+		MMSS_DSI_PHY_PLL_DIV_FRAC_START1,
+		vco_calc.dec_start2, vco_calc.div_frac_start1);
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL4,
+		MMSS_DSI_PHY_PLL_DIV_FRAC_START2,
+		MMSS_DSI_PHY_PLL_DIV_FRAC_START3,
+		vco_calc.div_frac_start2, vco_calc.div_frac_start3);
+	/* Method 2 - Auto PLL calibration */
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL7,
+		MMSS_DSI_PHY_PLL_PLL_VCO_TUNE,
+		MMSS_DSI_PHY_PLL_PLLLOCK_CMP_EN,
+		0, 0x0D);
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL8,
+		MMSS_DSI_PHY_PLL_POST_DIVIDER_CONTROL,
+		MMSS_DSI_PHY_PLL_RESETSM_CNTRL3,
+		0xF0, 0x07);
+
+	/*
+	 * RESETSM_CTRL3 has to be set for 12 times (6 reg writes),
+	 * Each register setting write 2 times, running in loop for 5
+	 * times (5 reg writes) and other two iterations are taken
+	 * care (one above and other in shadow_bypass
+	 */
+	for (rem = 0; rem < 5; rem++) {
+		MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+				MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL9 + (4 * rem),
+				MMSS_DSI_PHY_PLL_RESETSM_CNTRL3,
+				MMSS_DSI_PHY_PLL_RESETSM_CNTRL3,
+				0x07, 0x07);
+	}
+
+	MDSS_DYN_PLL_REG_W(dsi_pll_res->dyn_pll_base,
+		MMSS_DSI_DYNAMIC_REFRESH_PLL_CTRL15,
+		MMSS_DSI_PHY_PLL_RESETSM_CNTRL3,
+		MMSS_DSI_PHY_PLL_RESETSM_CNTRL3,
+		0x03, 0x03);
+
+	wmb();
+	return 0;
+}
+
 unsigned long pll_20nm_vco_get_rate(struct clk *c)
 {
 	u64 vco_rate, multiplier = (1 << 20);
@@ -595,7 +881,7 @@ unsigned long pll_20nm_vco_get_rate(struct clk *c)
 			MMSS_DSI_PHY_PLL_DEC_START2) & BIT(0)) << 7;
 	dec_start |= (MDSS_PLL_REG_R(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_DEC_START1) & 0x7f);
-	pr_debug("%s: dec_start = 0x%x\n", __func__, dec_start);
+	pr_debug("dec_start = 0x%x\n", dec_start);
 
 	div_frac_start = (MDSS_PLL_REG_R(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_DIV_FRAC_START3) & 0x3f) << 14;
@@ -603,13 +889,11 @@ unsigned long pll_20nm_vco_get_rate(struct clk *c)
 			MMSS_DSI_PHY_PLL_DIV_FRAC_START2) & 0x7f) << 7;
 	div_frac_start |= MDSS_PLL_REG_R(dsi_pll_res->pll_base,
 			MMSS_DSI_PHY_PLL_DIV_FRAC_START1) & 0x7f;
-	pr_debug("%s: div_frac_start = 0x%x\n",
-			__func__, div_frac_start);
+	pr_debug("div_frac_start = 0x%x\n", div_frac_start);
 
 	vco_rate = ref_clk * 2 * dec_start;
 	vco_rate += ((ref_clk * 2 * div_frac_start) / multiplier);
-	pr_debug("%s: returning vco rate = %lu\n",
-			__func__, (unsigned long)vco_rate);
+	pr_debug("returning vco rate = %lu\n", (unsigned long)vco_rate);
 
 	mdss_pll_resource_enable(dsi_pll_res, false);
 
diff --git a/drivers/clk/msm/mdss/mdss-dsi-pll-20nm.c b/drivers/clk/msm/mdss/mdss-dsi-pll-20nm.c
index e7074a6ef778..10d8b074a813 100644
--- a/drivers/clk/msm/mdss/mdss-dsi-pll-20nm.c
+++ b/drivers/clk/msm/mdss/mdss-dsi-pll-20nm.c
@@ -31,6 +31,10 @@ static struct clk_ops pixel_clk_src_ops;
 static struct clk_ops byte_clk_src_ops;
 static struct clk_ops ndiv_clk_ops;
 
+static struct clk_ops shadow_pixel_clk_src_ops;
+static struct clk_ops shadow_byte_clk_src_ops;
+static struct clk_ops clk_ops_gen_mux_dsi;
+
 static int vco_set_rate_20nm(struct clk *c, unsigned long rate)
 {
 	int rc;
@@ -49,6 +53,22 @@ static int vco_set_rate_20nm(struct clk *c, unsigned long rate)
 	return rc;
 }
 
+static int shadow_vco_set_rate_20nm(struct clk *c, unsigned long rate)
+{
+	int rc;
+	struct dsi_pll_vco_clk *vco = to_vco_clk(c);
+	struct mdss_pll_resources *dsi_pll_res = vco->priv;
+
+	if (!dsi_pll_res->resource_enable) {
+		pr_err("PLL resources disabled. Dynamic fps invalid\n");
+		return -EINVAL;
+	}
+
+	rc = shadow_pll_20nm_vco_set_rate(vco, rate);
+
+	return rc;
+}
+
 static int dsi_pll_enable_seq_8994(struct mdss_pll_resources *dsi_pll_res)
 {
 	int rc = 0;
@@ -77,13 +97,17 @@ static int dsi_pll_enable_seq_8994(struct mdss_pll_resources *dsi_pll_res)
 		rc = -EINVAL;
 	} else {
 		pr_debug("DSI PLL Lock success\n");
+		/*
+		*  Following cached registers are useful when
+		*  dynamic refresh feature is enabled.
+		*/
+		dsi_cache_trim_codes(dsi_pll_res);
 	}
 
 	return rc;
 }
 
 /* Op structures */
-
 static struct clk_ops clk_ops_dsi_vco = {
 	.set_rate = vco_set_rate_20nm,
 	.round_rate = pll_20nm_vco_round_rate,
@@ -92,7 +116,6 @@ static struct clk_ops clk_ops_dsi_vco = {
 	.unprepare = pll_20nm_vco_unprepare,
 };
 
-
 static struct clk_div_ops fixed_hr_oclk2_div_ops = {
 	.set_div = fixed_hr_oclk2_set_div,
 	.get_div = fixed_hr_oclk2_get_div,
@@ -113,6 +136,42 @@ static struct clk_mux_ops bypass_lp_div_mux_ops = {
 	.get_mux_sel = get_bypass_lp_div_mux_sel,
 };
 
+static struct clk_ops shadow_clk_ops_dsi_vco = {
+	.set_rate = shadow_vco_set_rate_20nm,
+	.round_rate = pll_20nm_vco_round_rate,
+	.handoff = pll_20nm_vco_handoff,
+};
+
+static struct clk_div_ops shadow_fixed_hr_oclk2_div_ops = {
+	.set_div = shadow_fixed_hr_oclk2_set_div,
+	.get_div = fixed_hr_oclk2_get_div,
+};
+
+static struct clk_div_ops shadow_ndiv_ops = {
+	.set_div = shadow_ndiv_set_div,
+	.get_div = ndiv_get_div,
+};
+
+static struct clk_div_ops shadow_hr_oclk3_div_ops = {
+	.set_div = shadow_hr_oclk3_set_div,
+	.get_div = hr_oclk3_get_div,
+};
+
+static struct clk_mux_ops shadow_bypass_lp_div_mux_ops = {
+	.set_mux_sel = set_shadow_bypass_lp_div_mux_sel,
+	.get_mux_sel = get_bypass_lp_div_mux_sel,
+};
+
+static struct clk_mux_ops mdss_byte_mux_ops = {
+	.set_mux_sel = set_mdss_byte_mux_sel,
+	.get_mux_sel = get_mdss_byte_mux_sel,
+};
+
+static struct clk_mux_ops mdss_pixel_mux_ops = {
+	.set_mux_sel = set_mdss_pixel_mux_sel,
+	.get_mux_sel = get_mdss_pixel_mux_sel,
+};
+
 static struct dsi_pll_vco_clk dsi_vco_clk_8994 = {
 	.ref_clk_rate = 19200000,
 	.min_rate = 1000000000,
@@ -126,6 +185,17 @@ static struct dsi_pll_vco_clk dsi_vco_clk_8994 = {
 	},
 };
 
+static struct dsi_pll_vco_clk shadow_dsi_vco_clk_8994 = {
+	.ref_clk_rate = 19200000,
+	.min_rate = 1000000000,
+	.max_rate = 2000000000,
+	.c = {
+		.dbg_name = "shadow_dsi_vco_clk_8994",
+		.ops = &shadow_clk_ops_dsi_vco,
+		CLK_INIT(shadow_dsi_vco_clk_8994.c),
+	},
+};
+
 static struct div_clk ndiv_clk_8994 = {
 	.data = {
 		.max_div = 15,
@@ -141,6 +211,21 @@ static struct div_clk ndiv_clk_8994 = {
 	},
 };
 
+static struct div_clk shadow_ndiv_clk_8994 = {
+	.data = {
+		.max_div = 15,
+		.min_div = 1,
+	},
+	.ops = &shadow_ndiv_ops,
+	.c = {
+		.parent = &shadow_dsi_vco_clk_8994.c,
+		.dbg_name = "shadow_ndiv_clk_8994",
+		.ops = &clk_ops_div,
+		.flags = CLKFLAG_NO_RATE_CACHE,
+		CLK_INIT(shadow_ndiv_clk_8994.c),
+	},
+};
+
 static struct div_clk indirect_path_div2_clk_8994 = {
 	.data = {
 		.div = 2,
@@ -156,6 +241,21 @@ static struct div_clk indirect_path_div2_clk_8994 = {
 	},
 };
 
+static struct div_clk shadow_indirect_path_div2_clk_8994 = {
+	.data = {
+		.div = 2,
+		.min_div = 2,
+		.max_div = 2,
+	},
+	.c = {
+		.parent = &shadow_ndiv_clk_8994.c,
+		.dbg_name = "shadow_indirect_path_div2_clk_8994",
+		.ops = &clk_ops_div,
+		.flags = CLKFLAG_NO_RATE_CACHE,
+		CLK_INIT(shadow_indirect_path_div2_clk_8994.c),
+	},
+};
+
 static struct div_clk hr_oclk3_div_clk_8994 = {
 	.data = {
 		.max_div = 255,
@@ -171,6 +271,21 @@ static struct div_clk hr_oclk3_div_clk_8994 = {
 	},
 };
 
+static struct div_clk shadow_hr_oclk3_div_clk_8994 = {
+	.data = {
+		.max_div = 255,
+		.min_div = 1,
+	},
+	.ops = &shadow_hr_oclk3_div_ops,
+	.c = {
+		.parent = &shadow_dsi_vco_clk_8994.c,
+		.dbg_name = "shadow_hr_oclk3_div_clk_8994",
+		.ops = &shadow_pixel_clk_src_ops,
+		.flags = CLKFLAG_NO_RATE_CACHE,
+		CLK_INIT(shadow_hr_oclk3_div_clk_8994.c),
+	},
+};
+
 static struct div_clk pixel_clk_src = {
 	.data = {
 		.div = 2,
@@ -186,6 +301,21 @@ static struct div_clk pixel_clk_src = {
 	},
 };
 
+static struct div_clk shadow_pixel_clk_src = {
+	.data = {
+		.div = 2,
+		.min_div = 2,
+		.max_div = 2,
+	},
+	.c = {
+		.parent = &shadow_hr_oclk3_div_clk_8994.c,
+		.dbg_name = "shadow_pixel_clk_src",
+		.ops = &clk_ops_div,
+		.flags = CLKFLAG_NO_RATE_CACHE,
+		CLK_INIT(shadow_pixel_clk_src.c),
+	},
+};
+
 static struct mux_clk bypass_lp_div_mux_8994 = {
 	.num_parents = 2,
 	.parents = (struct clk_src[]){
@@ -201,6 +331,21 @@ static struct mux_clk bypass_lp_div_mux_8994 = {
 	},
 };
 
+static struct mux_clk shadow_bypass_lp_div_mux_8994 = {
+	.num_parents = 2,
+	.parents = (struct clk_src[]){
+		{&shadow_dsi_vco_clk_8994.c, 0},
+		{&shadow_indirect_path_div2_clk_8994.c, 1},
+	},
+	.ops = &shadow_bypass_lp_div_mux_ops,
+	.c = {
+		.parent = &shadow_dsi_vco_clk_8994.c,
+		.dbg_name = "shadow_bypass_lp_div_mux_8994",
+		.ops = &clk_ops_gen_mux,
+		CLK_INIT(shadow_bypass_lp_div_mux_8994.c),
+	},
+};
+
 static struct div_clk fixed_hr_oclk2_div_clk_8994 = {
 	.ops = &fixed_hr_oclk2_div_ops,
 	.data = {
@@ -215,6 +360,20 @@ static struct div_clk fixed_hr_oclk2_div_clk_8994 = {
 	},
 };
 
+static struct div_clk shadow_fixed_hr_oclk2_div_clk_8994 = {
+	.ops = &shadow_fixed_hr_oclk2_div_ops,
+	.data = {
+		.min_div = 4,
+		.max_div = 4,
+	},
+	.c = {
+		.parent = &shadow_bypass_lp_div_mux_8994.c,
+		.dbg_name = "shadow_fixed_hr_oclk2_div_clk_8994",
+		.ops = &shadow_byte_clk_src_ops,
+		CLK_INIT(shadow_fixed_hr_oclk2_div_clk_8994.c),
+	},
+};
+
 static struct div_clk byte_clk_src = {
 	.data = {
 		.div = 2,
@@ -229,7 +388,53 @@ static struct div_clk byte_clk_src = {
 	},
 };
 
+static struct div_clk shadow_byte_clk_src = {
+	.data = {
+		.div = 2,
+		.min_div = 2,
+		.max_div = 2,
+	},
+	.c = {
+		.parent = &shadow_fixed_hr_oclk2_div_clk_8994.c,
+		.dbg_name = "shadow_byte_clk_src",
+		.ops = &clk_ops_div,
+		CLK_INIT(shadow_byte_clk_src.c),
+	},
+};
+
+static struct mux_clk mdss_pixel_clk_mux = {
+	.num_parents = 2,
+	.parents = (struct clk_src[]) {
+		{&pixel_clk_src.c, 0},
+		{&shadow_pixel_clk_src.c, 1},
+	},
+	.ops = &mdss_pixel_mux_ops,
+	.c = {
+		.parent = &pixel_clk_src.c,
+		.dbg_name = "mdss_pixel_clk_mux",
+		.ops = &clk_ops_gen_mux,
+		CLK_INIT(mdss_pixel_clk_mux.c),
+	}
+};
+
+static struct mux_clk mdss_byte_clk_mux = {
+	.num_parents = 2,
+	.parents = (struct clk_src[]) {
+		{&byte_clk_src.c, 0},
+		{&shadow_byte_clk_src.c, 1},
+	},
+	.ops = &mdss_byte_mux_ops,
+	.c = {
+		.parent = &byte_clk_src.c,
+		.dbg_name = "mdss_byte_clk_mux",
+		.ops = &clk_ops_gen_mux_dsi,
+		CLK_INIT(mdss_byte_clk_mux.c),
+	}
+};
+
 static struct clk_lookup mdss_dsi_pllcc_8994[] = {
+	CLK_LIST(mdss_pixel_clk_mux),
+	CLK_LIST(mdss_byte_clk_mux),
 	CLK_LIST(pixel_clk_src),
 	CLK_LIST(byte_clk_src),
 	CLK_LIST(fixed_hr_oclk2_div_clk_8994),
@@ -238,6 +443,14 @@ static struct clk_lookup mdss_dsi_pllcc_8994[] = {
 	CLK_LIST(indirect_path_div2_clk_8994),
 	CLK_LIST(ndiv_clk_8994),
 	CLK_LIST(dsi_vco_clk_8994),
+	CLK_LIST(shadow_pixel_clk_src),
+	CLK_LIST(shadow_byte_clk_src),
+	CLK_LIST(shadow_fixed_hr_oclk2_div_clk_8994),
+	CLK_LIST(shadow_bypass_lp_div_mux_8994),
+	CLK_LIST(shadow_hr_oclk3_div_clk_8994),
+	CLK_LIST(shadow_indirect_path_div2_clk_8994),
+	CLK_LIST(shadow_ndiv_clk_8994),
+	CLK_LIST(shadow_dsi_vco_clk_8994),
 };
 
 int dsi_pll_clock_register_20nm(struct platform_device *pdev,
@@ -264,6 +477,16 @@ int dsi_pll_clock_register_20nm(struct platform_device *pdev,
 	fixed_hr_oclk2_div_clk_8994.priv = pll_res;
 	hr_oclk3_div_clk_8994.priv = pll_res;
 	dsi_vco_clk_8994.priv = pll_res;
+
+	shadow_byte_clk_src.priv = pll_res;
+	shadow_pixel_clk_src.priv = pll_res;
+	shadow_bypass_lp_div_mux_8994.priv = pll_res;
+	shadow_indirect_path_div2_clk_8994.priv = pll_res;
+	shadow_ndiv_clk_8994.priv = pll_res;
+	shadow_fixed_hr_oclk2_div_clk_8994.priv = pll_res;
+	shadow_hr_oclk3_div_clk_8994.priv = pll_res;
+	shadow_dsi_vco_clk_8994.priv = pll_res;
+
 	pll_res->vco_delay = VCO_DELAY_USEC;
 
 	/* Set clock source operations */
@@ -279,6 +502,16 @@ int dsi_pll_clock_register_20nm(struct platform_device *pdev,
 	bypass_lp_div_mux_clk_ops = clk_ops_gen_mux;
 	bypass_lp_div_mux_clk_ops.prepare = dsi_pll_mux_prepare;
 
+	clk_ops_gen_mux_dsi = clk_ops_gen_mux;
+	clk_ops_gen_mux_dsi.round_rate = parent_round_rate;
+	clk_ops_gen_mux_dsi.set_rate = parent_set_rate;
+
+	shadow_pixel_clk_src_ops = clk_ops_slave_div;
+	shadow_pixel_clk_src_ops.prepare = dsi_pll_div_prepare;
+
+	shadow_byte_clk_src_ops = clk_ops_div;
+	shadow_byte_clk_src_ops.prepare = dsi_pll_div_prepare;
+
 	if (pll_res->target_id == MDSS_PLL_TARGET_8994) {
 		rc = of_msm_clock_register(pdev->dev.of_node,
 			mdss_dsi_pllcc_8994, ARRAY_SIZE(mdss_dsi_pllcc_8994));
diff --git a/drivers/clk/msm/mdss/mdss-dsi-pll.h b/drivers/clk/msm/mdss/mdss-dsi-pll.h
index 1060bc436044..1ee2b5810f9c 100644
--- a/drivers/clk/msm/mdss/mdss-dsi-pll.h
+++ b/drivers/clk/msm/mdss/mdss-dsi-pll.h
@@ -79,6 +79,8 @@ void vco_unprepare(struct clk *c);
 
 /* APIs for 20nm PHY PLL */
 int pll_20nm_vco_set_rate(struct dsi_pll_vco_clk *vco, unsigned long rate);
+int shadow_pll_20nm_vco_set_rate(struct dsi_pll_vco_clk *vco,
+				unsigned long rate);
 long pll_20nm_vco_round_rate(struct clk *c, unsigned long rate);
 enum handoff pll_20nm_vco_handoff(struct clk *c);
 int pll_20nm_vco_prepare(struct clk *c);
@@ -86,12 +88,22 @@ void pll_20nm_vco_unprepare(struct clk *c);
 int dsi_20nm_pll_lock_status(struct mdss_pll_resources *dsi_pll_res);
 
 int set_bypass_lp_div_mux_sel(struct mux_clk *clk, int sel);
+int set_shadow_bypass_lp_div_mux_sel(struct mux_clk *clk, int sel);
 int get_bypass_lp_div_mux_sel(struct mux_clk *clk);
 int fixed_hr_oclk2_set_div(struct div_clk *clk, int div);
+int shadow_fixed_hr_oclk2_set_div(struct div_clk *clk, int div);
 int fixed_hr_oclk2_get_div(struct div_clk *clk);
 int hr_oclk3_set_div(struct div_clk *clk, int div);
+int shadow_hr_oclk3_set_div(struct div_clk *clk, int div);
 int hr_oclk3_get_div(struct div_clk *clk);
 int ndiv_set_div(struct div_clk *clk, int div);
+int shadow_ndiv_set_div(struct div_clk *clk, int div);
 int ndiv_get_div(struct div_clk *clk);
 
+int set_mdss_pixel_mux_sel(struct mux_clk *clk, int sel);
+int get_mdss_pixel_mux_sel(struct mux_clk *clk);
+int set_mdss_byte_mux_sel(struct mux_clk *clk, int sel);
+int get_mdss_byte_mux_sel(struct mux_clk *clk);
+void dsi_cache_trim_codes(struct mdss_pll_resources *dsi_pll_res);
+
 #endif
diff --git a/drivers/clk/msm/mdss/mdss-pll.h b/drivers/clk/msm/mdss/mdss-pll.h
index 1bb8f7306dcd..fe1ed8b97d70 100644
--- a/drivers/clk/msm/mdss/mdss-pll.h
+++ b/drivers/clk/msm/mdss/mdss-pll.h
@@ -18,7 +18,14 @@
 
 #define MDSS_PLL_REG_W(base, offset, data)	\
 				writel_relaxed((data), (base) + (offset))
-#define MDSS_PLL_REG_R(base, offset)		readl_relaxed((base) + (offset))
+#define MDSS_PLL_REG_R(base, offset)	readl_relaxed((base) + (offset))
+
+#define PLL_CALC_DATA(addr0, addr1, data0, data1)      \
+	(((data1) << 24) | (((addr1)/4) << 16) | ((data0) << 8) | ((addr0)/4))
+
+#define MDSS_DYN_PLL_REG_W(base, offset, addr0, addr1, data0, data1)   \
+		writel_relaxed(PLL_CALC_DATA(addr0, addr1, data0, data1), \
+			(base) + (offset))
 
 enum {
 	MDSS_DSI_PLL_LPM,
@@ -90,6 +97,27 @@ struct mdss_pll_resources {
 	 * feature is disabled.
 	 */
 	bool		handoff_resources;
+
+	/*
+	 * caching the pll trim codes in the case of dynamic refresh
+	 */
+	int		cache_pll_trim_codes[5];
+
+	/*
+	 * for maintaining the status of saving trim codes
+	 */
+	bool		reg_upd;
+};
+
+struct mdss_pll_vco_calc {
+	s32 div_frac_start1;
+	s32 div_frac_start2;
+	s32 div_frac_start3;
+	s64 dec_start1;
+	s64 dec_start2;
+	s64 pll_plllock_cmp1;
+	s64 pll_plllock_cmp2;
+	s64 pll_plllock_cmp3;
 };
 
 int mdss_pll_resource_enable(struct mdss_pll_resources *pll_res, bool enable);