clk: msm: clock-local: Add RCG support for DP pixel source

Add a new RCG op specific for the DP pixel clock source.

CRs-Fixed: 1028725
Change-Id: I65dcac9f4d17d30dfa1a00f4edabef33a3d75c6a
Signed-off-by: Deepak Katragadda <dkatraga@codeaurora.org>

3 files changed, 52 insertions, 0 deletions

diff --git a/arch/arm64/Kconfig.platforms b/arch/arm64/Kconfig.platforms
index 4e38de7ddd3e..084b942d102d 100644
--- a/arch/arm64/Kconfig.platforms
+++ b/arch/arm64/Kconfig.platforms
@@ -56,6 +56,7 @@ config ARCH_QCOM
     	select SOC_BUS
 	select MSM_IRQ
 	select THERMAL_WRITABLE_TRIPS
+	select RATIONAL
 	help
 	  This enables support for the ARMv8 based Qualcomm chipsets.
 
diff --git a/drivers/clk/msm/clock-local2.c b/drivers/clk/msm/clock-local2.c
index 55fa76046def..6cf53c78d4d6 100644
--- a/drivers/clk/msm/clock-local2.c
+++ b/drivers/clk/msm/clock-local2.c
@@ -21,6 +21,7 @@
 #include <linux/io.h>
 #include <linux/spinlock.h>
 #include <linux/delay.h>
+#include <linux/rational.h>
 #include <linux/clk.h>
 #include <linux/clk/msm-clk-provider.h>
 #include <linux/clk/msm-clk.h>
@@ -1735,6 +1736,46 @@ static struct clk *edp_clk_get_parent(struct clk *c)
 	return freq->src_clk;
 }
 
+static int rcg_clk_set_rate_dp(struct clk *clk, unsigned long rate)
+{
+	struct rcg_clk *rcg = to_rcg_clk(clk);
+	struct clk_freq_tbl *freq_tbl = rcg->current_freq;
+	unsigned long src_rate;
+	unsigned long num, den, flags;
+
+	src_rate = clk_get_rate(clk->parent);
+	if (src_rate <= 0) {
+		pr_err("Invalid RCG parent rate\n");
+		return -EINVAL;
+	}
+
+	rational_best_approximation(src_rate, rate,
+			(unsigned long)(1 << 16) - 1,
+			(unsigned long)(1 << 16) - 1, &den, &num);
+
+	if (!num || !den) {
+		pr_err("Invalid MN values derived for requested rate %lu\n",
+							rate);
+		return -EINVAL;
+	}
+
+	freq_tbl->div_src_val &= ~BM(4, 0);
+	if (num == den) {
+		freq_tbl->m_val = 0;
+		freq_tbl->n_val = 0;
+	} else {
+		freq_tbl->m_val = num;
+		freq_tbl->n_val = ~(den - num);
+		freq_tbl->d_val = ~den;
+	}
+
+	spin_lock_irqsave(&local_clock_reg_lock, flags);
+	if (!is_same_rcg_config(rcg, freq_tbl, true))
+		__set_rate_mnd(rcg, freq_tbl);
+	spin_unlock_irqrestore(&local_clock_reg_lock, flags);
+	return 0;
+}
+
 static int gate_clk_enable(struct clk *c)
 {
 	unsigned long flags;
@@ -2291,6 +2332,15 @@ struct clk_ops clk_ops_rcg_edp = {
 	.list_registers = rcg_hid_clk_list_registers,
 };
 
+struct clk_ops clk_ops_rcg_dp = {
+	.enable = rcg_clk_enable,
+	.disable = rcg_clk_disable,
+	.set_rate = rcg_clk_set_rate_dp,
+	.list_rate = rcg_clk_list_rate,
+	.handoff = pixel_rcg_handoff,
+	.list_registers = rcg_mnd_clk_list_registers,
+};
+
 struct clk_ops clk_ops_branch = {
 	.enable = branch_clk_enable,
 	.prepare = branch_clk_prepare,
diff --git a/include/soc/qcom/clock-local2.h b/include/soc/qcom/clock-local2.h
index 7f785cf4d3a2..4f2fa36e920f 100644
--- a/include/soc/qcom/clock-local2.h
+++ b/include/soc/qcom/clock-local2.h
@@ -255,6 +255,7 @@ extern struct clk_ops clk_ops_branch_hw_ctl;
 extern struct clk_ops clk_ops_vote;
 extern struct clk_ops clk_ops_rcg_hdmi;
 extern struct clk_ops clk_ops_rcg_edp;
+extern struct clk_ops clk_ops_rcg_dp;
 extern struct clk_ops clk_ops_byte;
 extern struct clk_ops clk_ops_pixel;
 extern struct clk_ops clk_ops_byte_multiparent;