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