1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (c) 2017, Linaro Limited 4 * Author: Georgi Djakov <georgi.djakov@linaro.org> 5 */ 6 7 #include <linux/bitops.h> 8 #include <linux/delay.h> 9 #include <linux/kernel.h> 10 #include <linux/regmap.h> 11 12 #include "clk-regmap-mux-div.h" 13 14 #define CMD_RCGR 0x0 15 #define CMD_RCGR_UPDATE BIT(0) 16 #define CMD_RCGR_DIRTY_CFG BIT(4) 17 #define CMD_RCGR_ROOT_OFF BIT(31) 18 #define CFG_RCGR 0x4 19 20 #define to_clk_regmap_mux_div(_hw) \ 21 container_of(to_clk_regmap(_hw), struct clk_regmap_mux_div, clkr) 22 23 int mux_div_set_src_div(struct clk_regmap_mux_div *md, u32 src, u32 div) 24 { 25 int ret, count; 26 u32 val, mask; 27 const char *name = clk_hw_get_name(&md->clkr.hw); 28 29 val = (div << md->hid_shift) | (src << md->src_shift); 30 mask = ((BIT(md->hid_width) - 1) << md->hid_shift) | 31 ((BIT(md->src_width) - 1) << md->src_shift); 32 33 ret = regmap_update_bits(md->clkr.regmap, CFG_RCGR + md->reg_offset, 34 mask, val); 35 if (ret) 36 return ret; 37 38 ret = regmap_update_bits(md->clkr.regmap, CMD_RCGR + md->reg_offset, 39 CMD_RCGR_UPDATE, CMD_RCGR_UPDATE); 40 if (ret) 41 return ret; 42 43 /* Wait for update to take effect */ 44 for (count = 500; count > 0; count--) { 45 ret = regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, 46 &val); 47 if (ret) 48 return ret; 49 if (!(val & CMD_RCGR_UPDATE)) 50 return 0; 51 udelay(1); 52 } 53 54 pr_err("%s: RCG did not update its configuration", name); 55 return -EBUSY; 56 } 57 EXPORT_SYMBOL_GPL(mux_div_set_src_div); 58 59 static void mux_div_get_src_div(struct clk_regmap_mux_div *md, u32 *src, 60 u32 *div) 61 { 62 u32 val, d, s; 63 const char *name = clk_hw_get_name(&md->clkr.hw); 64 65 regmap_read(md->clkr.regmap, CMD_RCGR + md->reg_offset, &val); 66 67 if (val & CMD_RCGR_DIRTY_CFG) { 68 pr_err("%s: RCG configuration is pending\n", name); 69 return; 70 } 71 72 regmap_read(md->clkr.regmap, CFG_RCGR + md->reg_offset, &val); 73 s = (val >> md->src_shift); 74 s &= BIT(md->src_width) - 1; 75 *src = s; 76 77 d = (val >> md->hid_shift); 78 d &= BIT(md->hid_width) - 1; 79 *div = d; 80 } 81 82 static inline bool is_better_rate(unsigned long req, unsigned long best, 83 unsigned long new) 84 { 85 return (req <= new && new < best) || (best < req && best < new); 86 } 87 88 static int mux_div_determine_rate(struct clk_hw *hw, 89 struct clk_rate_request *req) 90 { 91 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); 92 unsigned int i, div, max_div; 93 unsigned long actual_rate, best_rate = 0; 94 unsigned long req_rate = req->rate; 95 96 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 97 struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i); 98 unsigned long parent_rate = clk_hw_get_rate(parent); 99 100 max_div = BIT(md->hid_width) - 1; 101 for (div = 1; div < max_div; div++) { 102 parent_rate = mult_frac(req_rate, div, 2); 103 parent_rate = clk_hw_round_rate(parent, parent_rate); 104 actual_rate = mult_frac(parent_rate, 2, div); 105 106 if (is_better_rate(req_rate, best_rate, actual_rate)) { 107 best_rate = actual_rate; 108 req->rate = best_rate; 109 req->best_parent_rate = parent_rate; 110 req->best_parent_hw = parent; 111 } 112 113 if (actual_rate < req_rate || best_rate <= req_rate) 114 break; 115 } 116 } 117 118 if (!best_rate) 119 return -EINVAL; 120 121 return 0; 122 } 123 124 static int __mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, 125 unsigned long prate, u32 src) 126 { 127 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); 128 int ret; 129 u32 div, max_div, best_src = 0, best_div = 0; 130 unsigned int i; 131 unsigned long actual_rate, best_rate = 0; 132 133 for (i = 0; i < clk_hw_get_num_parents(hw); i++) { 134 struct clk_hw *parent = clk_hw_get_parent_by_index(hw, i); 135 unsigned long parent_rate = clk_hw_get_rate(parent); 136 137 max_div = BIT(md->hid_width) - 1; 138 for (div = 1; div < max_div; div++) { 139 parent_rate = mult_frac(rate, div, 2); 140 parent_rate = clk_hw_round_rate(parent, parent_rate); 141 actual_rate = mult_frac(parent_rate, 2, div); 142 143 if (is_better_rate(rate, best_rate, actual_rate)) { 144 best_rate = actual_rate; 145 best_src = md->parent_map[i]; 146 best_div = div - 1; 147 } 148 149 if (actual_rate < rate || best_rate <= rate) 150 break; 151 } 152 } 153 154 ret = mux_div_set_src_div(md, best_src, best_div); 155 if (!ret) { 156 md->div = best_div; 157 md->src = best_src; 158 } 159 160 return ret; 161 } 162 163 static u8 mux_div_get_parent(struct clk_hw *hw) 164 { 165 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); 166 const char *name = clk_hw_get_name(hw); 167 u32 i, div, src = 0; 168 169 mux_div_get_src_div(md, &src, &div); 170 171 for (i = 0; i < clk_hw_get_num_parents(hw); i++) 172 if (src == md->parent_map[i]) 173 return i; 174 175 pr_err("%s: Can't find parent with src %d\n", name, src); 176 return 0; 177 } 178 179 static int mux_div_set_parent(struct clk_hw *hw, u8 index) 180 { 181 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); 182 183 return mux_div_set_src_div(md, md->parent_map[index], md->div); 184 } 185 186 static int mux_div_set_rate(struct clk_hw *hw, 187 unsigned long rate, unsigned long prate) 188 { 189 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); 190 191 return __mux_div_set_rate_and_parent(hw, rate, prate, md->src); 192 } 193 194 static int mux_div_set_rate_and_parent(struct clk_hw *hw, unsigned long rate, 195 unsigned long prate, u8 index) 196 { 197 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); 198 199 return __mux_div_set_rate_and_parent(hw, rate, prate, 200 md->parent_map[index]); 201 } 202 203 static unsigned long mux_div_recalc_rate(struct clk_hw *hw, unsigned long prate) 204 { 205 struct clk_regmap_mux_div *md = to_clk_regmap_mux_div(hw); 206 u32 div, src; 207 int i, num_parents = clk_hw_get_num_parents(hw); 208 const char *name = clk_hw_get_name(hw); 209 210 mux_div_get_src_div(md, &src, &div); 211 for (i = 0; i < num_parents; i++) 212 if (src == md->parent_map[i]) { 213 struct clk_hw *p = clk_hw_get_parent_by_index(hw, i); 214 unsigned long parent_rate = clk_hw_get_rate(p); 215 216 return mult_frac(parent_rate, 2, div + 1); 217 } 218 219 pr_err("%s: Can't find parent %d\n", name, src); 220 return 0; 221 } 222 223 const struct clk_ops clk_regmap_mux_div_ops = { 224 .get_parent = mux_div_get_parent, 225 .set_parent = mux_div_set_parent, 226 .set_rate = mux_div_set_rate, 227 .set_rate_and_parent = mux_div_set_rate_and_parent, 228 .determine_rate = mux_div_determine_rate, 229 .recalc_rate = mux_div_recalc_rate, 230 }; 231 EXPORT_SYMBOL_GPL(clk_regmap_mux_div_ops); 232