237 lines · c
1// SPDX-License-Identifier: GPL-2.0-or-later2/*3 * Copyright (C) 2016 Free Electrons4 * Copyright (C) 2016 NextThing Co5 *6 * Maxime Ripard <maxime.ripard@free-electrons.com>7 */8 9#include <linux/clk-provider.h>10#include <linux/io.h>11 12#include "sun4i_hdmi.h"13 14struct sun4i_tmds {15 struct clk_hw hw;16 struct sun4i_hdmi *hdmi;17 18 u8 div_offset;19};20 21static inline struct sun4i_tmds *hw_to_tmds(struct clk_hw *hw)22{23 return container_of(hw, struct sun4i_tmds, hw);24}25 26 27static unsigned long sun4i_tmds_calc_divider(unsigned long rate,28 unsigned long parent_rate,29 u8 div_offset,30 u8 *div,31 bool *half)32{33 unsigned long best_rate = 0;34 u8 best_m = 0, m;35 bool is_double = false;36 37 for (m = div_offset ?: 1; m < (16 + div_offset); m++) {38 u8 d;39 40 for (d = 1; d < 3; d++) {41 unsigned long tmp_rate;42 43 tmp_rate = parent_rate / m / d;44 45 if (tmp_rate > rate)46 continue;47 48 if (!best_rate ||49 (rate - tmp_rate) < (rate - best_rate)) {50 best_rate = tmp_rate;51 best_m = m;52 is_double = (d == 2) ? true : false;53 }54 }55 }56 57 if (div && half) {58 *div = best_m;59 *half = is_double;60 }61 62 return best_rate;63}64 65 66static int sun4i_tmds_determine_rate(struct clk_hw *hw,67 struct clk_rate_request *req)68{69 struct sun4i_tmds *tmds = hw_to_tmds(hw);70 struct clk_hw *parent = NULL;71 unsigned long best_parent = 0;72 unsigned long rate = req->rate;73 int best_div = 1, best_half = 1;74 int i, j, p;75 76 /*77 * We only consider PLL3, since the TCON is very likely to be78 * clocked from it, and to have the same rate than our HDMI79 * clock, so we should not need to do anything.80 */81 82 for (p = 0; p < clk_hw_get_num_parents(hw); p++) {83 parent = clk_hw_get_parent_by_index(hw, p);84 if (!parent)85 continue;86 87 for (i = 1; i < 3; i++) {88 for (j = tmds->div_offset ?: 1;89 j < (16 + tmds->div_offset); j++) {90 unsigned long ideal = rate * i * j;91 unsigned long rounded;92 93 rounded = clk_hw_round_rate(parent, ideal);94 95 if (rounded == ideal) {96 best_parent = rounded;97 best_half = i;98 best_div = j;99 goto out;100 }101 102 if (!best_parent ||103 abs(rate - rounded / i / j) <104 abs(rate - best_parent / best_half /105 best_div)) {106 best_parent = rounded;107 best_half = i;108 best_div = j;109 }110 }111 }112 }113 114 if (!parent)115 return -EINVAL;116 117out: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 125static 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 143static 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 168static 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 178static 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 194static 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 203int 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