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/regmap.h> 15 16 #include "sun4i_hdmi.h" 17 18 struct sun4i_ddc { 19 struct clk_hw hw; 20 struct sun4i_hdmi *hdmi; 21 struct regmap_field *reg; 22 u8 pre_div; 23 u8 m_offset; 24 }; 25 26 static inline struct sun4i_ddc *hw_to_ddc(struct clk_hw *hw) 27 { 28 return container_of(hw, struct sun4i_ddc, hw); 29 } 30 31 static unsigned long sun4i_ddc_calc_divider(unsigned long rate, 32 unsigned long parent_rate, 33 const u8 pre_div, 34 const u8 m_offset, 35 u8 *m, u8 *n) 36 { 37 unsigned long best_rate = 0; 38 u8 best_m = 0, best_n = 0, _m, _n; 39 40 for (_m = 0; _m < 8; _m++) { 41 for (_n = 0; _n < 8; _n++) { 42 unsigned long tmp_rate; 43 44 tmp_rate = (((parent_rate / pre_div) / 10) >> _n) / 45 (_m + m_offset); 46 47 if (tmp_rate > rate) 48 continue; 49 50 if (abs(rate - tmp_rate) < abs(rate - best_rate)) { 51 best_rate = tmp_rate; 52 best_m = _m; 53 best_n = _n; 54 } 55 } 56 } 57 58 if (m && n) { 59 *m = best_m; 60 *n = best_n; 61 } 62 63 return best_rate; 64 } 65 66 static long sun4i_ddc_round_rate(struct clk_hw *hw, unsigned long rate, 67 unsigned long *prate) 68 { 69 struct sun4i_ddc *ddc = hw_to_ddc(hw); 70 71 return sun4i_ddc_calc_divider(rate, *prate, ddc->pre_div, 72 ddc->m_offset, NULL, NULL); 73 } 74 75 static unsigned long sun4i_ddc_recalc_rate(struct clk_hw *hw, 76 unsigned long parent_rate) 77 { 78 struct sun4i_ddc *ddc = hw_to_ddc(hw); 79 unsigned int reg; 80 u8 m, n; 81 82 regmap_field_read(ddc->reg, ®); 83 m = (reg >> 3) & 0xf; 84 n = reg & 0x7; 85 86 return (((parent_rate / ddc->pre_div) / 10) >> n) / 87 (m + ddc->m_offset); 88 } 89 90 static int sun4i_ddc_set_rate(struct clk_hw *hw, unsigned long rate, 91 unsigned long parent_rate) 92 { 93 struct sun4i_ddc *ddc = hw_to_ddc(hw); 94 u8 div_m, div_n; 95 96 sun4i_ddc_calc_divider(rate, parent_rate, ddc->pre_div, 97 ddc->m_offset, &div_m, &div_n); 98 99 regmap_field_write(ddc->reg, 100 SUN4I_HDMI_DDC_CLK_M(div_m) | 101 SUN4I_HDMI_DDC_CLK_N(div_n)); 102 103 return 0; 104 } 105 106 static const struct clk_ops sun4i_ddc_ops = { 107 .recalc_rate = sun4i_ddc_recalc_rate, 108 .round_rate = sun4i_ddc_round_rate, 109 .set_rate = sun4i_ddc_set_rate, 110 }; 111 112 int sun4i_ddc_create(struct sun4i_hdmi *hdmi, struct clk *parent) 113 { 114 struct clk_init_data init; 115 struct sun4i_ddc *ddc; 116 const char *parent_name; 117 118 parent_name = __clk_get_name(parent); 119 if (!parent_name) 120 return -ENODEV; 121 122 ddc = devm_kzalloc(hdmi->dev, sizeof(*ddc), GFP_KERNEL); 123 if (!ddc) 124 return -ENOMEM; 125 126 ddc->reg = devm_regmap_field_alloc(hdmi->dev, hdmi->regmap, 127 hdmi->variant->ddc_clk_reg); 128 if (IS_ERR(ddc->reg)) 129 return PTR_ERR(ddc->reg); 130 131 init.name = "hdmi-ddc"; 132 init.ops = &sun4i_ddc_ops; 133 init.parent_names = &parent_name; 134 init.num_parents = 1; 135 136 ddc->hdmi = hdmi; 137 ddc->hw.init = &init; 138 ddc->pre_div = hdmi->variant->ddc_clk_pre_divider; 139 ddc->m_offset = hdmi->variant->ddc_clk_m_offset; 140 141 hdmi->ddc_clk = devm_clk_register(hdmi->dev, &ddc->hw); 142 if (IS_ERR(hdmi->ddc_clk)) 143 return PTR_ERR(hdmi->ddc_clk); 144 145 return 0; 146 } 147