1 /* 2 * Copyright (c) 2013 NVIDIA CORPORATION. All rights reserved. 3 * 4 * This program is free software; you can redistribute it and/or modify it 5 * under the terms and conditions of the GNU General Public License, 6 * version 2, as published by the Free Software Foundation. 7 * 8 * This program is distributed in the hope it will be useful, but WITHOUT 9 * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or 10 * FITNESS FOR A PARTICULAR PURPOSE. See the GNU General Public License for 11 * more details. 12 * 13 * You should have received a copy of the GNU General Public License 14 * along with this program. If not, see <http://www.gnu.org/licenses/>. 15 */ 16 17 #include <linux/clk.h> 18 #include <linux/clk-provider.h> 19 #include <linux/err.h> 20 #include <linux/slab.h> 21 22 #define to_clk_composite(_hw) container_of(_hw, struct clk_composite, hw) 23 24 static u8 clk_composite_get_parent(struct clk_hw *hw) 25 { 26 struct clk_composite *composite = to_clk_composite(hw); 27 const struct clk_ops *mux_ops = composite->mux_ops; 28 struct clk_hw *mux_hw = composite->mux_hw; 29 30 __clk_hw_set_clk(mux_hw, hw); 31 32 return mux_ops->get_parent(mux_hw); 33 } 34 35 static int clk_composite_set_parent(struct clk_hw *hw, u8 index) 36 { 37 struct clk_composite *composite = to_clk_composite(hw); 38 const struct clk_ops *mux_ops = composite->mux_ops; 39 struct clk_hw *mux_hw = composite->mux_hw; 40 41 __clk_hw_set_clk(mux_hw, hw); 42 43 return mux_ops->set_parent(mux_hw, index); 44 } 45 46 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw, 47 unsigned long parent_rate) 48 { 49 struct clk_composite *composite = to_clk_composite(hw); 50 const struct clk_ops *rate_ops = composite->rate_ops; 51 struct clk_hw *rate_hw = composite->rate_hw; 52 53 __clk_hw_set_clk(rate_hw, hw); 54 55 return rate_ops->recalc_rate(rate_hw, parent_rate); 56 } 57 58 static int clk_composite_determine_rate(struct clk_hw *hw, 59 struct clk_rate_request *req) 60 { 61 struct clk_composite *composite = to_clk_composite(hw); 62 const struct clk_ops *rate_ops = composite->rate_ops; 63 const struct clk_ops *mux_ops = composite->mux_ops; 64 struct clk_hw *rate_hw = composite->rate_hw; 65 struct clk_hw *mux_hw = composite->mux_hw; 66 struct clk_hw *parent; 67 unsigned long parent_rate; 68 long tmp_rate, best_rate = 0; 69 unsigned long rate_diff; 70 unsigned long best_rate_diff = ULONG_MAX; 71 long rate; 72 int i; 73 74 if (rate_hw && rate_ops && rate_ops->determine_rate) { 75 __clk_hw_set_clk(rate_hw, hw); 76 return rate_ops->determine_rate(rate_hw, req); 77 } else if (rate_hw && rate_ops && rate_ops->round_rate && 78 mux_hw && mux_ops && mux_ops->set_parent) { 79 req->best_parent_hw = NULL; 80 81 if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) { 82 parent = clk_hw_get_parent(mux_hw); 83 req->best_parent_hw = parent; 84 req->best_parent_rate = clk_hw_get_rate(parent); 85 86 rate = rate_ops->round_rate(rate_hw, req->rate, 87 &req->best_parent_rate); 88 if (rate < 0) 89 return rate; 90 91 req->rate = rate; 92 return 0; 93 } 94 95 for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) { 96 parent = clk_hw_get_parent_by_index(mux_hw, i); 97 if (!parent) 98 continue; 99 100 parent_rate = clk_hw_get_rate(parent); 101 102 tmp_rate = rate_ops->round_rate(rate_hw, req->rate, 103 &parent_rate); 104 if (tmp_rate < 0) 105 continue; 106 107 rate_diff = abs(req->rate - tmp_rate); 108 109 if (!rate_diff || !req->best_parent_hw 110 || best_rate_diff > rate_diff) { 111 req->best_parent_hw = parent; 112 req->best_parent_rate = parent_rate; 113 best_rate_diff = rate_diff; 114 best_rate = tmp_rate; 115 } 116 117 if (!rate_diff) 118 return 0; 119 } 120 121 req->rate = best_rate; 122 return 0; 123 } else if (mux_hw && mux_ops && mux_ops->determine_rate) { 124 __clk_hw_set_clk(mux_hw, hw); 125 return mux_ops->determine_rate(mux_hw, req); 126 } else { 127 pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n"); 128 return -EINVAL; 129 } 130 } 131 132 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate, 133 unsigned long *prate) 134 { 135 struct clk_composite *composite = to_clk_composite(hw); 136 const struct clk_ops *rate_ops = composite->rate_ops; 137 struct clk_hw *rate_hw = composite->rate_hw; 138 139 __clk_hw_set_clk(rate_hw, hw); 140 141 return rate_ops->round_rate(rate_hw, rate, prate); 142 } 143 144 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate, 145 unsigned long parent_rate) 146 { 147 struct clk_composite *composite = to_clk_composite(hw); 148 const struct clk_ops *rate_ops = composite->rate_ops; 149 struct clk_hw *rate_hw = composite->rate_hw; 150 151 __clk_hw_set_clk(rate_hw, hw); 152 153 return rate_ops->set_rate(rate_hw, rate, parent_rate); 154 } 155 156 static int clk_composite_is_enabled(struct clk_hw *hw) 157 { 158 struct clk_composite *composite = to_clk_composite(hw); 159 const struct clk_ops *gate_ops = composite->gate_ops; 160 struct clk_hw *gate_hw = composite->gate_hw; 161 162 __clk_hw_set_clk(gate_hw, hw); 163 164 return gate_ops->is_enabled(gate_hw); 165 } 166 167 static int clk_composite_enable(struct clk_hw *hw) 168 { 169 struct clk_composite *composite = to_clk_composite(hw); 170 const struct clk_ops *gate_ops = composite->gate_ops; 171 struct clk_hw *gate_hw = composite->gate_hw; 172 173 __clk_hw_set_clk(gate_hw, hw); 174 175 return gate_ops->enable(gate_hw); 176 } 177 178 static void clk_composite_disable(struct clk_hw *hw) 179 { 180 struct clk_composite *composite = to_clk_composite(hw); 181 const struct clk_ops *gate_ops = composite->gate_ops; 182 struct clk_hw *gate_hw = composite->gate_hw; 183 184 __clk_hw_set_clk(gate_hw, hw); 185 186 gate_ops->disable(gate_hw); 187 } 188 189 struct clk *clk_register_composite(struct device *dev, const char *name, 190 const char * const *parent_names, int num_parents, 191 struct clk_hw *mux_hw, const struct clk_ops *mux_ops, 192 struct clk_hw *rate_hw, const struct clk_ops *rate_ops, 193 struct clk_hw *gate_hw, const struct clk_ops *gate_ops, 194 unsigned long flags) 195 { 196 struct clk *clk; 197 struct clk_init_data init; 198 struct clk_composite *composite; 199 struct clk_ops *clk_composite_ops; 200 201 composite = kzalloc(sizeof(*composite), GFP_KERNEL); 202 if (!composite) 203 return ERR_PTR(-ENOMEM); 204 205 init.name = name; 206 init.flags = flags | CLK_IS_BASIC; 207 init.parent_names = parent_names; 208 init.num_parents = num_parents; 209 210 clk_composite_ops = &composite->ops; 211 212 if (mux_hw && mux_ops) { 213 if (!mux_ops->get_parent) { 214 clk = ERR_PTR(-EINVAL); 215 goto err; 216 } 217 218 composite->mux_hw = mux_hw; 219 composite->mux_ops = mux_ops; 220 clk_composite_ops->get_parent = clk_composite_get_parent; 221 if (mux_ops->set_parent) 222 clk_composite_ops->set_parent = clk_composite_set_parent; 223 if (mux_ops->determine_rate) 224 clk_composite_ops->determine_rate = clk_composite_determine_rate; 225 } 226 227 if (rate_hw && rate_ops) { 228 if (!rate_ops->recalc_rate) { 229 clk = ERR_PTR(-EINVAL); 230 goto err; 231 } 232 clk_composite_ops->recalc_rate = clk_composite_recalc_rate; 233 234 if (rate_ops->determine_rate) 235 clk_composite_ops->determine_rate = 236 clk_composite_determine_rate; 237 else if (rate_ops->round_rate) 238 clk_composite_ops->round_rate = 239 clk_composite_round_rate; 240 241 /* .set_rate requires either .round_rate or .determine_rate */ 242 if (rate_ops->set_rate) { 243 if (rate_ops->determine_rate || rate_ops->round_rate) 244 clk_composite_ops->set_rate = 245 clk_composite_set_rate; 246 else 247 WARN(1, "%s: missing round_rate op is required\n", 248 __func__); 249 } 250 251 composite->rate_hw = rate_hw; 252 composite->rate_ops = rate_ops; 253 } 254 255 if (gate_hw && gate_ops) { 256 if (!gate_ops->is_enabled || !gate_ops->enable || 257 !gate_ops->disable) { 258 clk = ERR_PTR(-EINVAL); 259 goto err; 260 } 261 262 composite->gate_hw = gate_hw; 263 composite->gate_ops = gate_ops; 264 clk_composite_ops->is_enabled = clk_composite_is_enabled; 265 clk_composite_ops->enable = clk_composite_enable; 266 clk_composite_ops->disable = clk_composite_disable; 267 } 268 269 init.ops = clk_composite_ops; 270 composite->hw.init = &init; 271 272 clk = clk_register(dev, &composite->hw); 273 if (IS_ERR(clk)) 274 goto err; 275 276 if (composite->mux_hw) 277 composite->mux_hw->clk = clk; 278 279 if (composite->rate_hw) 280 composite->rate_hw->clk = clk; 281 282 if (composite->gate_hw) 283 composite->gate_hw->clk = clk; 284 285 return clk; 286 287 err: 288 kfree(composite); 289 return clk; 290 } 291