[PATCH] clk: meson: vid-pll-div: added meson_vid_pll_div_ops support to enable vid_pll_div to meet clock setting requirements, especially for late chip

[Yu Tu <yu.tu@amlogic.com>]

The previous chip only provides "ro_ops" for the vid_pll_div clock,
which is not satisfied with the operation requirements of the later
chip for this clock, so the ops that can be set for the clock is added.

Signed-off-by: Yu Tu <yu.tu@amlogic.com>
---
 drivers/clk/meson/vid-pll-div.c | 59 +++++++++++++++++++++++++++++++++
 drivers/clk/meson/vid-pll-div.h |  1 +
 2 files changed, 60 insertions(+)

diff --git a/drivers/clk/meson/vid-pll-div.c b/drivers/clk/meson/vid-pll-div.c
index daff235bc763..e75fa6f75efe 100644
--- a/drivers/clk/meson/vid-pll-div.c
+++ b/drivers/clk/meson/vid-pll-div.c
@@ -89,6 +89,65 @@ static unsigned long meson_vid_pll_div_recalc_rate(struct clk_hw *hw,
 	return DIV_ROUND_UP_ULL(parent_rate * div->multiplier, div->divider);
 }
 
+static int meson_vid_pll_div_determine_rate(struct clk_hw *hw,
+					    struct clk_rate_request *req)
+{
+	unsigned long best = 0, now = 0;
+	unsigned int i, best_i = 0;
+
+	for (i = 0 ; i < ARRAY_SIZE(vid_pll_div_table) ; ++i) {
+		now = DIV_ROUND_CLOSEST_ULL(req->best_parent_rate *
+					    vid_pll_div_table[i].multiplier,
+					    vid_pll_div_table[i].divider);
+		if (req->rate == now) {
+			return 0;
+		} else if (abs(now - req->rate) < abs(best - req->rate)) {
+			best = now;
+			best_i = i;
+		}
+	}
+
+	if (best_i < ARRAY_SIZE(vid_pll_div_table))
+		req->rate = DIV_ROUND_CLOSEST_ULL(req->best_parent_rate *
+						  vid_pll_div_table[best_i].multiplier,
+						  vid_pll_div_table[best_i].divider);
+	else
+		req->rate = meson_vid_pll_div_recalc_rate(hw, req->best_parent_rate);
+
+	return 0;
+}
+
+static int meson_vid_pll_div_set_rate(struct clk_hw *hw, unsigned long rate,
+				      unsigned long parent_rate)
+{
+	struct clk_regmap *clk = to_clk_regmap(hw);
+	struct meson_vid_pll_div_data *pll_div = meson_vid_pll_div_data(clk);
+	int i;
+
+	for (i = 0 ; i < ARRAY_SIZE(vid_pll_div_table) ; ++i) {
+		if (DIV_ROUND_CLOSEST_ULL(parent_rate * vid_pll_div_table[i].multiplier,
+					  vid_pll_div_table[i].divider) == rate) {
+			meson_parm_write(clk->map, &pll_div->val, vid_pll_div_table[i].shift_val);
+			meson_parm_write(clk->map, &pll_div->sel, vid_pll_div_table[i].shift_sel);
+			break;
+		}
+	}
+
+	if (i >= ARRAY_SIZE(vid_pll_div_table)) {
+		pr_debug("%s: Invalid rate value for vid_pll_div\n", __func__);
+		return -EINVAL;
+	}
+
+	return 0;
+}
+
+const struct clk_ops meson_vid_pll_div_ops = {
+	.recalc_rate	= meson_vid_pll_div_recalc_rate,
+	.determine_rate	= meson_vid_pll_div_determine_rate,
+	.set_rate	= meson_vid_pll_div_set_rate,
+};
+EXPORT_SYMBOL_GPL(meson_vid_pll_div_ops);
+
 const struct clk_ops meson_vid_pll_div_ro_ops = {
 	.recalc_rate	= meson_vid_pll_div_recalc_rate,
 };
diff --git a/drivers/clk/meson/vid-pll-div.h b/drivers/clk/meson/vid-pll-div.h
index c0128e33ccf9..3ab729b85fde 100644
--- a/drivers/clk/meson/vid-pll-div.h
+++ b/drivers/clk/meson/vid-pll-div.h
@@ -16,5 +16,6 @@ struct meson_vid_pll_div_data {
 };
 
 extern const struct clk_ops meson_vid_pll_div_ro_ops;
+extern const struct clk_ops meson_vid_pll_div_ops;
 
 #endif /* __MESON_VID_PLL_DIV_H */

base-commit: 8a9fbf00acfeeeaac8efab8091bb464bd71b70ea
-- 
2.33.1