1 /* 2 * Copyright (C) 2016 Free Electrons 3 * Copyright (C) 2016 NextThing Co 4 * 5 * Maxime Ripard <maxime.ripard@free-electrons.com> 6 * 7 * This program is free software; you can redistribute it and/or 8 * modify it under the terms of the GNU General Public License as 9 * published by the Free Software Foundation; either version 2 of 10 * the License, or (at your option) any later version. 11 */ 12 13 #include <linux/clk-provider.h> 14 15 #include "sun4i_hdmi.h" 16 17 struct sun4i_tmds { 18 struct clk_hw hw; 19 struct sun4i_hdmi *hdmi; 20 21 u8 div_offset; 22 }; 23 24 static inline struct sun4i_tmds *hw_to_tmds(struct clk_hw *hw) 25 { 26 return container_of(hw, struct sun4i_tmds, hw); 27 } 28 29 30 static unsigned long sun4i_tmds_calc_divider(unsigned long rate, 31 unsigned long parent_rate, 32 u8 div_offset, 33 u8 *div, 34 bool *half) 35 { 36 unsigned long best_rate = 0; 37 u8 best_m = 0, m; 38 bool is_double; 39 40 for (m = div_offset ?: 1; m < (16 + div_offset); m++) { 41 u8 d; 42 43 for (d = 1; d < 3; d++) { 44 unsigned long tmp_rate; 45 46 tmp_rate = parent_rate / m / d; 47 48 if (tmp_rate > rate) 49 continue; 50 51 if (!best_rate || 52 (rate - tmp_rate) < (rate - best_rate)) { 53 best_rate = tmp_rate; 54 best_m = m; 55 is_double = d; 56 } 57 } 58 } 59 60 if (div && half) { 61 *div = best_m; 62 *half = is_double; 63 } 64 65 return best_rate; 66 } 67 68 69 static int sun4i_tmds_determine_rate(struct clk_hw *hw, 70 struct clk_rate_request *req) 71 { 72 struct sun4i_tmds *tmds = hw_to_tmds(hw); 73 struct clk_hw *parent = NULL; 74 unsigned long best_parent = 0; 75 unsigned long rate = req->rate; 76 int best_div = 1, best_half = 1; 77 int i, j, p; 78 79 /* 80 * We only consider PLL3, since the TCON is very likely to be 81 * clocked from it, and to have the same rate than our HDMI 82 * clock, so we should not need to do anything. 83 */ 84 85 for (p = 0; p < clk_hw_get_num_parents(hw); p++) { 86 parent = clk_hw_get_parent_by_index(hw, p); 87 if (!parent) 88 continue; 89 90 for (i = 1; i < 3; i++) { 91 for (j = tmds->div_offset ?: 1; 92 j < (16 + tmds->div_offset); j++) { 93 unsigned long ideal = rate * i * j; 94 unsigned long rounded; 95 96 rounded = clk_hw_round_rate(parent, ideal); 97 98 if (rounded == ideal) { 99 best_parent = rounded; 100 best_half = i; 101 best_div = j; 102 goto out; 103 } 104 105 if (!best_parent || 106 abs(rate - rounded / i / j) < 107 abs(rate - best_parent / best_half / 108 best_div)) { 109 best_parent = rounded; 110 best_half = i; 111 best_div = j; 112 } 113 } 114 } 115 } 116 117 if (!parent) 118 return -EINVAL; 119 120 out: 121 req->rate = best_parent / best_half / best_div; 122 req->best_parent_rate = best_parent; 123 req->best_parent_hw = parent; 124 125 return 0; 126 } 127 128 static unsigned long sun4i_tmds_recalc_rate(struct clk_hw *hw, 129 unsigned long parent_rate) 130 { 131 struct sun4i_tmds *tmds = hw_to_tmds(hw); 132 u32 reg; 133 134 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); 135 if (reg & SUN4I_HDMI_PAD_CTRL1_HALVE_CLK) 136 parent_rate /= 2; 137 138 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); 139 reg = ((reg >> 4) & 0xf) + tmds->div_offset; 140 if (!reg) 141 reg = 1; 142 143 return parent_rate / reg; 144 } 145 146 static int sun4i_tmds_set_rate(struct clk_hw *hw, unsigned long rate, 147 unsigned long parent_rate) 148 { 149 struct sun4i_tmds *tmds = hw_to_tmds(hw); 150 bool half; 151 u32 reg; 152 u8 div; 153 154 sun4i_tmds_calc_divider(rate, parent_rate, tmds->div_offset, 155 &div, &half); 156 157 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); 158 reg &= ~SUN4I_HDMI_PAD_CTRL1_HALVE_CLK; 159 if (half) 160 reg |= SUN4I_HDMI_PAD_CTRL1_HALVE_CLK; 161 writel(reg, tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); 162 163 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); 164 reg &= ~SUN4I_HDMI_PLL_CTRL_DIV_MASK; 165 writel(reg | SUN4I_HDMI_PLL_CTRL_DIV(div - tmds->div_offset), 166 tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); 167 168 return 0; 169 } 170 171 static u8 sun4i_tmds_get_parent(struct clk_hw *hw) 172 { 173 struct sun4i_tmds *tmds = hw_to_tmds(hw); 174 u32 reg; 175 176 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); 177 return ((reg & SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_MASK) >> 178 SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_SHIFT); 179 } 180 181 static int sun4i_tmds_set_parent(struct clk_hw *hw, u8 index) 182 { 183 struct sun4i_tmds *tmds = hw_to_tmds(hw); 184 u32 reg; 185 186 if (index > 1) 187 return -EINVAL; 188 189 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); 190 reg &= ~SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_MASK; 191 writel(reg | SUN4I_HDMI_PLL_DBG0_TMDS_PARENT(index), 192 tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); 193 194 return 0; 195 } 196 197 static const struct clk_ops sun4i_tmds_ops = { 198 .determine_rate = sun4i_tmds_determine_rate, 199 .recalc_rate = sun4i_tmds_recalc_rate, 200 .set_rate = sun4i_tmds_set_rate, 201 202 .get_parent = sun4i_tmds_get_parent, 203 .set_parent = sun4i_tmds_set_parent, 204 }; 205 206 int sun4i_tmds_create(struct sun4i_hdmi *hdmi) 207 { 208 struct clk_init_data init; 209 struct sun4i_tmds *tmds; 210 const char *parents[2]; 211 212 parents[0] = __clk_get_name(hdmi->pll0_clk); 213 if (!parents[0]) 214 return -ENODEV; 215 216 parents[1] = __clk_get_name(hdmi->pll1_clk); 217 if (!parents[1]) 218 return -ENODEV; 219 220 tmds = devm_kzalloc(hdmi->dev, sizeof(*tmds), GFP_KERNEL); 221 if (!tmds) 222 return -ENOMEM; 223 224 init.name = "hdmi-tmds"; 225 init.ops = &sun4i_tmds_ops; 226 init.parent_names = parents; 227 init.num_parents = 2; 228 init.flags = CLK_SET_RATE_PARENT; 229 230 tmds->hdmi = hdmi; 231 tmds->hw.init = &init; 232 tmds->div_offset = hdmi->variant->tmds_clk_div_offset; 233 234 hdmi->tmds_clk = devm_clk_register(hdmi->dev, &tmds->hw); 235 if (IS_ERR(hdmi->tmds_clk)) 236 return PTR_ERR(hdmi->tmds_clk); 237 238 return 0; 239 } 240