143 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/regmap.h>11 12#include "sun4i_hdmi.h"13 14struct 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 22static inline struct sun4i_ddc *hw_to_ddc(struct clk_hw *hw)23{24 return container_of(hw, struct sun4i_ddc, hw);25}26 27static 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 62static 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 71static 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, ®);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 86static 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 102static 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 108int 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