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