1 // SPDX-License-Identifier: GPL-2.0-or-later
2 /*
3  * Copyright (C) 2016 Free Electrons
4  * Copyright (C) 2016 NextThing Co
5  *
6  * Maxime Ripard <maxime.ripard@free-electrons.com>
7  */
8 
9 #include <linux/clk-provider.h>
10 #include <linux/regmap.h>
11 
12 #include "sun4i_hdmi.h"
13 
14 struct sun4i_ddc {
15 	struct clk_hw		hw;
16 	struct sun4i_hdmi	*hdmi;
17 	struct regmap_field	*reg;
18 	u8			pre_div;
19 	u8			m_offset;
20 };
21 
hw_to_ddc(struct clk_hw * hw)22 static inline struct sun4i_ddc *hw_to_ddc(struct clk_hw *hw)
23 {
24 	return container_of(hw, struct sun4i_ddc, hw);
25 }
26 
sun4i_ddc_calc_divider(unsigned long rate,unsigned long parent_rate,const u8 pre_div,const u8 m_offset,u8 * m,u8 * n)27 static unsigned long sun4i_ddc_calc_divider(unsigned long rate,
28 					    unsigned long parent_rate,
29 					    const u8 pre_div,
30 					    const u8 m_offset,
31 					    u8 *m, u8 *n)
32 {
33 	unsigned long best_rate = 0;
34 	u8 best_m = 0, best_n = 0, _m, _n;
35 
36 	for (_m = 0; _m < 16; _m++) {
37 		for (_n = 0; _n < 8; _n++) {
38 			unsigned long tmp_rate;
39 
40 			tmp_rate = (((parent_rate / pre_div) / 10) >> _n) /
41 				(_m + m_offset);
42 
43 			if (tmp_rate > rate)
44 				continue;
45 
46 			if (abs(rate - tmp_rate) < abs(rate - best_rate)) {
47 				best_rate = tmp_rate;
48 				best_m = _m;
49 				best_n = _n;
50 			}
51 		}
52 	}
53 
54 	if (m && n) {
55 		*m = best_m;
56 		*n = best_n;
57 	}
58 
59 	return best_rate;
60 }
61 
sun4i_ddc_round_rate(struct clk_hw * hw,unsigned long rate,unsigned long * prate)62 static long sun4i_ddc_round_rate(struct clk_hw *hw, unsigned long rate,
63 				 unsigned long *prate)
64 {
65 	struct sun4i_ddc *ddc = hw_to_ddc(hw);
66 
67 	return sun4i_ddc_calc_divider(rate, *prate, ddc->pre_div,
68 				      ddc->m_offset, NULL, NULL);
69 }
70 
sun4i_ddc_recalc_rate(struct clk_hw * hw,unsigned long parent_rate)71 static unsigned long sun4i_ddc_recalc_rate(struct clk_hw *hw,
72 					    unsigned long parent_rate)
73 {
74 	struct sun4i_ddc *ddc = hw_to_ddc(hw);
75 	unsigned int reg;
76 	u8 m, n;
77 
78 	regmap_field_read(ddc->reg, &reg);
79 	m = (reg >> 3) & 0xf;
80 	n = reg & 0x7;
81 
82 	return (((parent_rate / ddc->pre_div) / 10) >> n) /
83 	       (m + ddc->m_offset);
84 }
85 
sun4i_ddc_set_rate(struct clk_hw * hw,unsigned long rate,unsigned long parent_rate)86 static int sun4i_ddc_set_rate(struct clk_hw *hw, unsigned long rate,
87 			      unsigned long parent_rate)
88 {
89 	struct sun4i_ddc *ddc = hw_to_ddc(hw);
90 	u8 div_m, div_n;
91 
92 	sun4i_ddc_calc_divider(rate, parent_rate, ddc->pre_div,
93 			       ddc->m_offset, &div_m, &div_n);
94 
95 	regmap_field_write(ddc->reg,
96 			   SUN4I_HDMI_DDC_CLK_M(div_m) |
97 			   SUN4I_HDMI_DDC_CLK_N(div_n));
98 
99 	return 0;
100 }
101 
102 static const struct clk_ops sun4i_ddc_ops = {
103 	.recalc_rate	= sun4i_ddc_recalc_rate,
104 	.round_rate	= sun4i_ddc_round_rate,
105 	.set_rate	= sun4i_ddc_set_rate,
106 };
107 
sun4i_ddc_create(struct sun4i_hdmi * hdmi,struct clk * parent)108 int sun4i_ddc_create(struct sun4i_hdmi *hdmi, struct clk *parent)
109 {
110 	struct clk_init_data init;
111 	struct sun4i_ddc *ddc;
112 	const char *parent_name;
113 
114 	parent_name = __clk_get_name(parent);
115 	if (!parent_name)
116 		return -ENODEV;
117 
118 	ddc = devm_kzalloc(hdmi->dev, sizeof(*ddc), GFP_KERNEL);
119 	if (!ddc)
120 		return -ENOMEM;
121 
122 	ddc->reg = devm_regmap_field_alloc(hdmi->dev, hdmi->regmap,
123 					   hdmi->variant->ddc_clk_reg);
124 	if (IS_ERR(ddc->reg))
125 		return PTR_ERR(ddc->reg);
126 
127 	init.name = "hdmi-ddc";
128 	init.ops = &sun4i_ddc_ops;
129 	init.parent_names = &parent_name;
130 	init.num_parents = 1;
131 
132 	ddc->hdmi = hdmi;
133 	ddc->hw.init = &init;
134 	ddc->pre_div = hdmi->variant->ddc_clk_pre_divider;
135 	ddc->m_offset = hdmi->variant->ddc_clk_m_offset;
136 
137 	hdmi->ddc_clk = devm_clk_register(hdmi->dev, &ddc->hw);
138 	if (IS_ERR(hdmi->ddc_clk))
139 		return PTR_ERR(hdmi->ddc_clk);
140 
141 	return 0;
142 }
143