Merge "clk: qcom: Add RCG support for DP pixel source"

2 files changed, 62 insertions, 0 deletions

diff --git a/drivers/clk/qcom/clk-rcg.h b/drivers/clk/qcom/clk-rcg.h
index 4589a3b6cec6..020bd351bbd8 100644
--- a/drivers/clk/qcom/clk-rcg.h
+++ b/drivers/clk/qcom/clk-rcg.h
@@ -187,5 +187,6 @@ extern const struct clk_ops clk_byte2_ops;
 extern const struct clk_ops clk_pixel_ops;
 extern const struct clk_ops clk_gfx3d_ops;
 extern const struct clk_ops clk_gfx3d_src_ops;
+extern const struct clk_ops clk_dp_ops;
 
 #endif
diff --git a/drivers/clk/qcom/clk-rcg2.c b/drivers/clk/qcom/clk-rcg2.c
index 48ff5ea00040..4104a238c088 100644
--- a/drivers/clk/qcom/clk-rcg2.c
+++ b/drivers/clk/qcom/clk-rcg2.c
@@ -20,6 +20,7 @@
 #include <linux/clk-provider.h>
 #include <linux/delay.h>
 #include <linux/regmap.h>
+#include <linux/rational.h>
 #include <linux/math64.h>
 
 #include <asm/div64.h>
@@ -847,6 +848,66 @@ const struct clk_ops clk_pixel_ops = {
 };
 EXPORT_SYMBOL_GPL(clk_pixel_ops);
 
+static int clk_dp_set_rate(struct clk_hw *hw, unsigned long rate,
+			unsigned long parent_rate)
+{
+	struct clk_rcg2 *rcg = to_clk_rcg2(hw);
+	struct freq_tbl f = { 0 };
+	unsigned long src_rate;
+	unsigned long num, den;
+	u32 mask = BIT(rcg->hid_width) - 1;
+	u32 hid_div;
+
+	src_rate = clk_get_rate(clk_hw_get_parent(hw)->clk);
+	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;
+	}
+
+	regmap_read(rcg->clkr.regmap, rcg->cmd_rcgr + CFG_REG, &hid_div);
+	f.pre_div = hid_div;
+	f.pre_div >>= CFG_SRC_DIV_SHIFT;
+	f.pre_div &= mask;
+
+	if (num == den) {
+		f.m = 0;
+		f.n = 0;
+	} else {
+		f.m = num;
+		f.n = den;
+	}
+
+	return clk_rcg2_configure(rcg, &f);
+}
+
+static int clk_dp_set_rate_and_parent(struct clk_hw *hw, unsigned long rate,
+		unsigned long parent_rate, u8 index)
+{
+	return clk_dp_set_rate(hw, rate, parent_rate);
+}
+
+const struct clk_ops clk_dp_ops = {
+	.is_enabled = clk_rcg2_is_enabled,
+	.get_parent = clk_rcg2_get_parent,
+	.set_parent = clk_rcg2_set_parent,
+	.recalc_rate = clk_rcg2_recalc_rate,
+	.set_rate = clk_dp_set_rate,
+	.set_rate_and_parent = clk_dp_set_rate_and_parent,
+	.determine_rate = clk_pixel_determine_rate,
+	.list_registers = clk_rcg2_list_registers,
+};
+EXPORT_SYMBOL_GPL(clk_dp_ops);
+
 static int clk_gfx3d_determine_rate(struct clk_hw *hw,
 				    struct clk_rate_request *req)
 {