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 (abs(rate - rounded / i) < 106 abs(rate - best_parent / best_div)) { 107 best_parent = rounded; 108 best_div = i; 109 } 110 } 111 } 112 } 113 114 if (!parent) 115 return -EINVAL; 116 117 out: 118 req->rate = best_parent / best_half / best_div; 119 req->best_parent_rate = best_parent; 120 req->best_parent_hw = parent; 121 122 return 0; 123 } 124 125 static unsigned long sun4i_tmds_recalc_rate(struct clk_hw *hw, 126 unsigned long parent_rate) 127 { 128 struct sun4i_tmds *tmds = hw_to_tmds(hw); 129 u32 reg; 130 131 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); 132 if (reg & SUN4I_HDMI_PAD_CTRL1_HALVE_CLK) 133 parent_rate /= 2; 134 135 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); 136 reg = ((reg >> 4) & 0xf) + tmds->div_offset; 137 if (!reg) 138 reg = 1; 139 140 return parent_rate / reg; 141 } 142 143 static int sun4i_tmds_set_rate(struct clk_hw *hw, unsigned long rate, 144 unsigned long parent_rate) 145 { 146 struct sun4i_tmds *tmds = hw_to_tmds(hw); 147 bool half; 148 u32 reg; 149 u8 div; 150 151 sun4i_tmds_calc_divider(rate, parent_rate, tmds->div_offset, 152 &div, &half); 153 154 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); 155 reg &= ~SUN4I_HDMI_PAD_CTRL1_HALVE_CLK; 156 if (half) 157 reg |= SUN4I_HDMI_PAD_CTRL1_HALVE_CLK; 158 writel(reg, tmds->hdmi->base + SUN4I_HDMI_PAD_CTRL1_REG); 159 160 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); 161 reg &= ~SUN4I_HDMI_PLL_CTRL_DIV_MASK; 162 writel(reg | SUN4I_HDMI_PLL_CTRL_DIV(div - tmds->div_offset), 163 tmds->hdmi->base + SUN4I_HDMI_PLL_CTRL_REG); 164 165 return 0; 166 } 167 168 static u8 sun4i_tmds_get_parent(struct clk_hw *hw) 169 { 170 struct sun4i_tmds *tmds = hw_to_tmds(hw); 171 u32 reg; 172 173 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); 174 return ((reg & SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_MASK) >> 175 SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_SHIFT); 176 } 177 178 static int sun4i_tmds_set_parent(struct clk_hw *hw, u8 index) 179 { 180 struct sun4i_tmds *tmds = hw_to_tmds(hw); 181 u32 reg; 182 183 if (index > 1) 184 return -EINVAL; 185 186 reg = readl(tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); 187 reg &= ~SUN4I_HDMI_PLL_DBG0_TMDS_PARENT_MASK; 188 writel(reg | SUN4I_HDMI_PLL_DBG0_TMDS_PARENT(index), 189 tmds->hdmi->base + SUN4I_HDMI_PLL_DBG0_REG); 190 191 return 0; 192 } 193 194 static const struct clk_ops sun4i_tmds_ops = { 195 .determine_rate = sun4i_tmds_determine_rate, 196 .recalc_rate = sun4i_tmds_recalc_rate, 197 .set_rate = sun4i_tmds_set_rate, 198 199 .get_parent = sun4i_tmds_get_parent, 200 .set_parent = sun4i_tmds_set_parent, 201 }; 202 203 int sun4i_tmds_create(struct sun4i_hdmi *hdmi) 204 { 205 struct clk_init_data init; 206 struct sun4i_tmds *tmds; 207 const char *parents[2]; 208 209 parents[0] = __clk_get_name(hdmi->pll0_clk); 210 if (!parents[0]) 211 return -ENODEV; 212 213 parents[1] = __clk_get_name(hdmi->pll1_clk); 214 if (!parents[1]) 215 return -ENODEV; 216 217 tmds = devm_kzalloc(hdmi->dev, sizeof(*tmds), GFP_KERNEL); 218 if (!tmds) 219 return -ENOMEM; 220 221 init.name = "hdmi-tmds"; 222 init.ops = &sun4i_tmds_ops; 223 init.parent_names = parents; 224 init.num_parents = 2; 225 init.flags = CLK_SET_RATE_PARENT; 226 227 tmds->hdmi = hdmi; 228 tmds->hw.init = &init; 229 tmds->div_offset = hdmi->variant->tmds_clk_div_offset; 230 231 hdmi->tmds_clk = devm_clk_register(hdmi->dev, &tmds->hw); 232 if (IS_ERR(hdmi->tmds_clk)) 233 return PTR_ERR(hdmi->tmds_clk); 234 235 return 0; 236 } 237