[PATCH V2] clk: meson: vid-pll-div: added meson_vid_pll_div_ops support

[Yu Tu]

Since the previous code only provides "ro_ops" for the vid_pll_div
clock. In fact, the clock can be set. So add "ops" that can set the
clock, especially for later chips like S4 SOC and so on.

Signed-off-by: Yu Tu <yu.tu@xxxxxxxxxxx>
---
 drivers/clk/meson/vid-pll-div.c | 67 +++++++++++++++++++++++++++++++++
 drivers/clk/meson/vid-pll-div.h |  3 ++
 2 files changed, 70 insertions(+)

diff --git a/drivers/clk/meson/vid-pll-div.c b/drivers/clk/meson/vid-pll-div.c
index daff235bc763..3c9015944f24 100644
--- a/drivers/clk/meson/vid-pll-div.c
+++ b/drivers/clk/meson/vid-pll-div.c
@@ -89,6 +89,73 @@ 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 parent_rate, best = 0, now = 0, rate;
+	unsigned long parent_rate_saved = req->best_parent_rate;
+	unsigned int i, best_i = 0;
+
+	for (i = 0 ; i < VID_PLL_DIV_TABLE_SIZE; ++i) {
+		rate = DIV_ROUND_CLOSEST_ULL(req->rate * vid_pll_div_table[i].divider,
+					     vid_pll_div_table[i].multiplier);
+		if (parent_rate_saved == rate) {
+			req->best_parent_rate = parent_rate_saved;
+			best_i = i;
+			break;
+		}
+
+		parent_rate = clk_hw_round_rate(req->best_parent_hw, rate);
+		now = DIV_ROUND_CLOSEST_ULL(parent_rate *
+					    vid_pll_div_table[i].multiplier,
+					    vid_pll_div_table[i].divider);
+		if (abs(now - req->rate) < abs(best - req->rate)) {
+			best = now;
+			best_i = i;
+			req->best_parent_rate = parent_rate;
+		}
+	}
+
+	req->rate = DIV_ROUND_CLOSEST_ULL(req->best_parent_rate *
+					  vid_pll_div_table[best_i].multiplier,
+					  vid_pll_div_table[best_i].divider);
+
+	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);
+	unsigned long best = 0, now = 0;
+	unsigned int i, best_i = 0;
+
+	for (i = 0 ; i < VID_PLL_DIV_TABLE_SIZE; ++i) {
+		now = DIV_ROUND_CLOSEST_ULL(parent_rate * vid_pll_div_table[i].multiplier,
+					    vid_pll_div_table[i].divider);
+		if (now == rate) {
+			best_i = i;
+			break;
+		} else if (abs(now - rate) < abs(best - rate)) {
+			best = now;
+			best_i = i;
+		}
+	}
+
+	meson_parm_write(clk->map, &pll_div->val, vid_pll_div_table[best_i].shift_val);
+	meson_parm_write(clk->map, &pll_div->sel, vid_pll_div_table[best_i].shift_sel);
+
+	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..bbccab340910 100644
--- a/drivers/clk/meson/vid-pll-div.h
+++ b/drivers/clk/meson/vid-pll-div.h
@@ -10,11 +10,14 @@
 #include <linux/clk-provider.h>
 #include "parm.h"
 
+#define VID_PLL_DIV_TABLE_SIZE		14
+
 struct meson_vid_pll_div_data {
 	struct parm val;
 	struct parm sel;
 };
 
 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 */
-- 
2.33.1