1 // SPDX-License-Identifier: GPL-2.0+
2 /*
3  * Copyright (C) 2018 Jernej Skrabec <jernej.skrabec@siol.net>
4  */
5 
6 #include <linux/clk-provider.h>
7 
8 #include "sun8i_dw_hdmi.h"
9 
10 struct sun8i_phy_clk {
11 	struct clk_hw		hw;
12 	struct sun8i_hdmi_phy	*phy;
13 };
14 
hw_to_phy_clk(struct clk_hw * hw)15 static inline struct sun8i_phy_clk *hw_to_phy_clk(struct clk_hw *hw)
16 {
17 	return container_of(hw, struct sun8i_phy_clk, hw);
18 }
19 
sun8i_phy_clk_determine_rate(struct clk_hw * hw,struct clk_rate_request * req)20 static int sun8i_phy_clk_determine_rate(struct clk_hw *hw,
21 					struct clk_rate_request *req)
22 {
23 	unsigned long rate = req->rate;
24 	unsigned long best_rate = 0;
25 	struct clk_hw *best_parent = NULL;
26 	struct clk_hw *parent;
27 	int best_div = 1;
28 	int i, p;
29 
30 	for (p = 0; p < clk_hw_get_num_parents(hw); p++) {
31 		parent = clk_hw_get_parent_by_index(hw, p);
32 		if (!parent)
33 			continue;
34 
35 		for (i = 1; i <= 16; i++) {
36 			unsigned long ideal = rate * i;
37 			unsigned long rounded;
38 
39 			rounded = clk_hw_round_rate(parent, ideal);
40 
41 			if (rounded == ideal) {
42 				best_rate = rounded;
43 				best_div = i;
44 				best_parent = parent;
45 				break;
46 			}
47 
48 			if (!best_rate ||
49 			    abs(rate - rounded / i) <
50 			    abs(rate - best_rate / best_div)) {
51 				best_rate = rounded;
52 				best_div = i;
53 				best_parent = parent;
54 			}
55 		}
56 
57 		if (best_rate / best_div == rate)
58 			break;
59 	}
60 
61 	req->rate = best_rate / best_div;
62 	req->best_parent_rate = best_rate;
63 	req->best_parent_hw = best_parent;
64 
65 	return 0;
66 }
67 
sun8i_phy_clk_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)68 static unsigned long sun8i_phy_clk_recalc_rate(struct clk_hw *hw,
69 					       unsigned long parent_rate)
70 {
71 	struct sun8i_phy_clk *priv = hw_to_phy_clk(hw);
72 	u32 reg;
73 
74 	regmap_read(priv->phy->regs, SUN8I_HDMI_PHY_PLL_CFG2_REG, &reg);
75 	reg = ((reg >> SUN8I_HDMI_PHY_PLL_CFG2_PREDIV_SHIFT) &
76 		SUN8I_HDMI_PHY_PLL_CFG2_PREDIV_MSK) + 1;
77 
78 	return parent_rate / reg;
79 }
80 
sun8i_phy_clk_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)81 static int sun8i_phy_clk_set_rate(struct clk_hw *hw, unsigned long rate,
82 				  unsigned long parent_rate)
83 {
84 	struct sun8i_phy_clk *priv = hw_to_phy_clk(hw);
85 	unsigned long best_rate = 0;
86 	u8 best_m = 0, m;
87 
88 	for (m = 1; m <= 16; m++) {
89 		unsigned long tmp_rate = parent_rate / m;
90 
91 		if (tmp_rate > rate)
92 			continue;
93 
94 		if (!best_rate ||
95 		    (rate - tmp_rate) < (rate - best_rate)) {
96 			best_rate = tmp_rate;
97 			best_m = m;
98 		}
99 	}
100 
101 	regmap_update_bits(priv->phy->regs, SUN8I_HDMI_PHY_PLL_CFG2_REG,
102 			   SUN8I_HDMI_PHY_PLL_CFG2_PREDIV_MSK,
103 			   SUN8I_HDMI_PHY_PLL_CFG2_PREDIV(best_m));
104 
105 	return 0;
106 }
107 
sun8i_phy_clk_get_parent(struct clk_hw * hw)108 static u8 sun8i_phy_clk_get_parent(struct clk_hw *hw)
109 {
110 	struct sun8i_phy_clk *priv = hw_to_phy_clk(hw);
111 	u32 reg;
112 
113 	regmap_read(priv->phy->regs, SUN8I_HDMI_PHY_PLL_CFG1_REG, &reg);
114 	reg = (reg & SUN8I_HDMI_PHY_PLL_CFG1_CKIN_SEL_MSK) >>
115 	      SUN8I_HDMI_PHY_PLL_CFG1_CKIN_SEL_SHIFT;
116 
117 	return reg;
118 }
119 
sun8i_phy_clk_set_parent(struct clk_hw * hw,u8 index)120 static int sun8i_phy_clk_set_parent(struct clk_hw *hw, u8 index)
121 {
122 	struct sun8i_phy_clk *priv = hw_to_phy_clk(hw);
123 
124 	if (index > 1)
125 		return -EINVAL;
126 
127 	regmap_update_bits(priv->phy->regs, SUN8I_HDMI_PHY_PLL_CFG1_REG,
128 			   SUN8I_HDMI_PHY_PLL_CFG1_CKIN_SEL_MSK,
129 			   index << SUN8I_HDMI_PHY_PLL_CFG1_CKIN_SEL_SHIFT);
130 
131 	return 0;
132 }
133 
134 static const struct clk_ops sun8i_phy_clk_ops = {
135 	.determine_rate	= sun8i_phy_clk_determine_rate,
136 	.recalc_rate	= sun8i_phy_clk_recalc_rate,
137 	.set_rate	= sun8i_phy_clk_set_rate,
138 
139 	.get_parent	= sun8i_phy_clk_get_parent,
140 	.set_parent	= sun8i_phy_clk_set_parent,
141 };
142 
sun8i_phy_clk_create(struct sun8i_hdmi_phy * phy,struct device * dev,bool second_parent)143 int sun8i_phy_clk_create(struct sun8i_hdmi_phy *phy, struct device *dev,
144 			 bool second_parent)
145 {
146 	struct clk_init_data init;
147 	struct sun8i_phy_clk *priv;
148 	const char *parents[2];
149 
150 	parents[0] = __clk_get_name(phy->clk_pll0);
151 	if (!parents[0])
152 		return -ENODEV;
153 
154 	if (second_parent) {
155 		parents[1] = __clk_get_name(phy->clk_pll1);
156 		if (!parents[1])
157 			return -ENODEV;
158 	}
159 
160 	priv = devm_kzalloc(dev, sizeof(*priv), GFP_KERNEL);
161 	if (!priv)
162 		return -ENOMEM;
163 
164 	init.name = "hdmi-phy-clk";
165 	init.ops = &sun8i_phy_clk_ops;
166 	init.parent_names = parents;
167 	init.num_parents = second_parent ? 2 : 1;
168 	init.flags = CLK_SET_RATE_PARENT;
169 
170 	priv->phy = phy;
171 	priv->hw.init = &init;
172 
173 	phy->clk_phy = devm_clk_register(dev, &priv->hw);
174 	if (IS_ERR(phy->clk_phy))
175 		return PTR_ERR(phy->clk_phy);
176 
177 	return 0;
178 }
179