1 // SPDX-License-Identifier: GPL-2.0 2 /* 3 * Copyright (c) 2013 NVIDIA CORPORATION. All rights reserved. 4 */ 5 6 #include <linux/clk.h> 7 #include <linux/clk-provider.h> 8 #include <linux/err.h> 9 #include <linux/slab.h> 10 11 static u8 clk_composite_get_parent(struct clk_hw *hw) 12 { 13 struct clk_composite *composite = to_clk_composite(hw); 14 const struct clk_ops *mux_ops = composite->mux_ops; 15 struct clk_hw *mux_hw = composite->mux_hw; 16 17 __clk_hw_set_clk(mux_hw, hw); 18 19 return mux_ops->get_parent(mux_hw); 20 } 21 22 static int clk_composite_set_parent(struct clk_hw *hw, u8 index) 23 { 24 struct clk_composite *composite = to_clk_composite(hw); 25 const struct clk_ops *mux_ops = composite->mux_ops; 26 struct clk_hw *mux_hw = composite->mux_hw; 27 28 __clk_hw_set_clk(mux_hw, hw); 29 30 return mux_ops->set_parent(mux_hw, index); 31 } 32 33 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw, 34 unsigned long parent_rate) 35 { 36 struct clk_composite *composite = to_clk_composite(hw); 37 const struct clk_ops *rate_ops = composite->rate_ops; 38 struct clk_hw *rate_hw = composite->rate_hw; 39 40 __clk_hw_set_clk(rate_hw, hw); 41 42 return rate_ops->recalc_rate(rate_hw, parent_rate); 43 } 44 45 static int clk_composite_determine_rate(struct clk_hw *hw, 46 struct clk_rate_request *req) 47 { 48 struct clk_composite *composite = to_clk_composite(hw); 49 const struct clk_ops *rate_ops = composite->rate_ops; 50 const struct clk_ops *mux_ops = composite->mux_ops; 51 struct clk_hw *rate_hw = composite->rate_hw; 52 struct clk_hw *mux_hw = composite->mux_hw; 53 struct clk_hw *parent; 54 unsigned long parent_rate; 55 long tmp_rate, best_rate = 0; 56 unsigned long rate_diff; 57 unsigned long best_rate_diff = ULONG_MAX; 58 long rate; 59 int i; 60 61 if (rate_hw && rate_ops && rate_ops->determine_rate) { 62 __clk_hw_set_clk(rate_hw, hw); 63 return rate_ops->determine_rate(rate_hw, req); 64 } else if (rate_hw && rate_ops && rate_ops->round_rate && 65 mux_hw && mux_ops && mux_ops->set_parent) { 66 req->best_parent_hw = NULL; 67 68 if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) { 69 parent = clk_hw_get_parent(mux_hw); 70 req->best_parent_hw = parent; 71 req->best_parent_rate = clk_hw_get_rate(parent); 72 73 rate = rate_ops->round_rate(rate_hw, req->rate, 74 &req->best_parent_rate); 75 if (rate < 0) 76 return rate; 77 78 req->rate = rate; 79 return 0; 80 } 81 82 for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) { 83 parent = clk_hw_get_parent_by_index(mux_hw, i); 84 if (!parent) 85 continue; 86 87 parent_rate = clk_hw_get_rate(parent); 88 89 tmp_rate = rate_ops->round_rate(rate_hw, req->rate, 90 &parent_rate); 91 if (tmp_rate < 0) 92 continue; 93 94 rate_diff = abs(req->rate - tmp_rate); 95 96 if (!rate_diff || !req->best_parent_hw 97 || best_rate_diff > rate_diff) { 98 req->best_parent_hw = parent; 99 req->best_parent_rate = parent_rate; 100 best_rate_diff = rate_diff; 101 best_rate = tmp_rate; 102 } 103 104 if (!rate_diff) 105 return 0; 106 } 107 108 req->rate = best_rate; 109 return 0; 110 } else if (mux_hw && mux_ops && mux_ops->determine_rate) { 111 __clk_hw_set_clk(mux_hw, hw); 112 return mux_ops->determine_rate(mux_hw, req); 113 } else { 114 pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n"); 115 return -EINVAL; 116 } 117 } 118 119 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate, 120 unsigned long *prate) 121 { 122 struct clk_composite *composite = to_clk_composite(hw); 123 const struct clk_ops *rate_ops = composite->rate_ops; 124 struct clk_hw *rate_hw = composite->rate_hw; 125 126 __clk_hw_set_clk(rate_hw, hw); 127 128 return rate_ops->round_rate(rate_hw, rate, prate); 129 } 130 131 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate, 132 unsigned long parent_rate) 133 { 134 struct clk_composite *composite = to_clk_composite(hw); 135 const struct clk_ops *rate_ops = composite->rate_ops; 136 struct clk_hw *rate_hw = composite->rate_hw; 137 138 __clk_hw_set_clk(rate_hw, hw); 139 140 return rate_ops->set_rate(rate_hw, rate, parent_rate); 141 } 142 143 static int clk_composite_set_rate_and_parent(struct clk_hw *hw, 144 unsigned long rate, 145 unsigned long parent_rate, 146 u8 index) 147 { 148 struct clk_composite *composite = to_clk_composite(hw); 149 const struct clk_ops *rate_ops = composite->rate_ops; 150 const struct clk_ops *mux_ops = composite->mux_ops; 151 struct clk_hw *rate_hw = composite->rate_hw; 152 struct clk_hw *mux_hw = composite->mux_hw; 153 unsigned long temp_rate; 154 155 __clk_hw_set_clk(rate_hw, hw); 156 __clk_hw_set_clk(mux_hw, hw); 157 158 temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate); 159 if (temp_rate > rate) { 160 rate_ops->set_rate(rate_hw, rate, parent_rate); 161 mux_ops->set_parent(mux_hw, index); 162 } else { 163 mux_ops->set_parent(mux_hw, index); 164 rate_ops->set_rate(rate_hw, rate, parent_rate); 165 } 166 167 return 0; 168 } 169 170 static int clk_composite_is_enabled(struct clk_hw *hw) 171 { 172 struct clk_composite *composite = to_clk_composite(hw); 173 const struct clk_ops *gate_ops = composite->gate_ops; 174 struct clk_hw *gate_hw = composite->gate_hw; 175 176 __clk_hw_set_clk(gate_hw, hw); 177 178 return gate_ops->is_enabled(gate_hw); 179 } 180 181 static int clk_composite_enable(struct clk_hw *hw) 182 { 183 struct clk_composite *composite = to_clk_composite(hw); 184 const struct clk_ops *gate_ops = composite->gate_ops; 185 struct clk_hw *gate_hw = composite->gate_hw; 186 187 __clk_hw_set_clk(gate_hw, hw); 188 189 return gate_ops->enable(gate_hw); 190 } 191 192 static void clk_composite_disable(struct clk_hw *hw) 193 { 194 struct clk_composite *composite = to_clk_composite(hw); 195 const struct clk_ops *gate_ops = composite->gate_ops; 196 struct clk_hw *gate_hw = composite->gate_hw; 197 198 __clk_hw_set_clk(gate_hw, hw); 199 200 gate_ops->disable(gate_hw); 201 } 202 203 struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name, 204 const char * const *parent_names, int num_parents, 205 struct clk_hw *mux_hw, const struct clk_ops *mux_ops, 206 struct clk_hw *rate_hw, const struct clk_ops *rate_ops, 207 struct clk_hw *gate_hw, const struct clk_ops *gate_ops, 208 unsigned long flags) 209 { 210 struct clk_hw *hw; 211 struct clk_init_data init; 212 struct clk_composite *composite; 213 struct clk_ops *clk_composite_ops; 214 int ret; 215 216 composite = kzalloc(sizeof(*composite), GFP_KERNEL); 217 if (!composite) 218 return ERR_PTR(-ENOMEM); 219 220 init.name = name; 221 init.flags = flags; 222 init.parent_names = parent_names; 223 init.num_parents = num_parents; 224 hw = &composite->hw; 225 226 clk_composite_ops = &composite->ops; 227 228 if (mux_hw && mux_ops) { 229 if (!mux_ops->get_parent) { 230 hw = ERR_PTR(-EINVAL); 231 goto err; 232 } 233 234 composite->mux_hw = mux_hw; 235 composite->mux_ops = mux_ops; 236 clk_composite_ops->get_parent = clk_composite_get_parent; 237 if (mux_ops->set_parent) 238 clk_composite_ops->set_parent = clk_composite_set_parent; 239 if (mux_ops->determine_rate) 240 clk_composite_ops->determine_rate = clk_composite_determine_rate; 241 } 242 243 if (rate_hw && rate_ops) { 244 if (!rate_ops->recalc_rate) { 245 hw = ERR_PTR(-EINVAL); 246 goto err; 247 } 248 clk_composite_ops->recalc_rate = clk_composite_recalc_rate; 249 250 if (rate_ops->determine_rate) 251 clk_composite_ops->determine_rate = 252 clk_composite_determine_rate; 253 else if (rate_ops->round_rate) 254 clk_composite_ops->round_rate = 255 clk_composite_round_rate; 256 257 /* .set_rate requires either .round_rate or .determine_rate */ 258 if (rate_ops->set_rate) { 259 if (rate_ops->determine_rate || rate_ops->round_rate) 260 clk_composite_ops->set_rate = 261 clk_composite_set_rate; 262 else 263 WARN(1, "%s: missing round_rate op is required\n", 264 __func__); 265 } 266 267 composite->rate_hw = rate_hw; 268 composite->rate_ops = rate_ops; 269 } 270 271 if (mux_hw && mux_ops && rate_hw && rate_ops) { 272 if (mux_ops->set_parent && rate_ops->set_rate) 273 clk_composite_ops->set_rate_and_parent = 274 clk_composite_set_rate_and_parent; 275 } 276 277 if (gate_hw && gate_ops) { 278 if (!gate_ops->is_enabled || !gate_ops->enable || 279 !gate_ops->disable) { 280 hw = ERR_PTR(-EINVAL); 281 goto err; 282 } 283 284 composite->gate_hw = gate_hw; 285 composite->gate_ops = gate_ops; 286 clk_composite_ops->is_enabled = clk_composite_is_enabled; 287 clk_composite_ops->enable = clk_composite_enable; 288 clk_composite_ops->disable = clk_composite_disable; 289 } 290 291 init.ops = clk_composite_ops; 292 composite->hw.init = &init; 293 294 ret = clk_hw_register(dev, hw); 295 if (ret) { 296 hw = ERR_PTR(ret); 297 goto err; 298 } 299 300 if (composite->mux_hw) 301 composite->mux_hw->clk = hw->clk; 302 303 if (composite->rate_hw) 304 composite->rate_hw->clk = hw->clk; 305 306 if (composite->gate_hw) 307 composite->gate_hw->clk = hw->clk; 308 309 return hw; 310 311 err: 312 kfree(composite); 313 return hw; 314 } 315 316 struct clk *clk_register_composite(struct device *dev, const char *name, 317 const char * const *parent_names, int num_parents, 318 struct clk_hw *mux_hw, const struct clk_ops *mux_ops, 319 struct clk_hw *rate_hw, const struct clk_ops *rate_ops, 320 struct clk_hw *gate_hw, const struct clk_ops *gate_ops, 321 unsigned long flags) 322 { 323 struct clk_hw *hw; 324 325 hw = clk_hw_register_composite(dev, name, parent_names, num_parents, 326 mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops, 327 flags); 328 if (IS_ERR(hw)) 329 return ERR_CAST(hw); 330 return hw->clk; 331 } 332 333 void clk_unregister_composite(struct clk *clk) 334 { 335 struct clk_composite *composite; 336 struct clk_hw *hw; 337 338 hw = __clk_get_hw(clk); 339 if (!hw) 340 return; 341 342 composite = to_clk_composite(hw); 343 344 clk_unregister(clk); 345 kfree(composite); 346 } 347