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 static u8 clk_composite_get_parent(struct clk_hw *hw) 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->get_parent(mux_hw); 31 } 32 33 static int clk_composite_set_parent(struct clk_hw *hw, u8 index) 34 { 35 struct clk_composite *composite = to_clk_composite(hw); 36 const struct clk_ops *mux_ops = composite->mux_ops; 37 struct clk_hw *mux_hw = composite->mux_hw; 38 39 __clk_hw_set_clk(mux_hw, hw); 40 41 return mux_ops->set_parent(mux_hw, index); 42 } 43 44 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw, 45 unsigned long parent_rate) 46 { 47 struct clk_composite *composite = to_clk_composite(hw); 48 const struct clk_ops *rate_ops = composite->rate_ops; 49 struct clk_hw *rate_hw = composite->rate_hw; 50 51 __clk_hw_set_clk(rate_hw, hw); 52 53 return rate_ops->recalc_rate(rate_hw, parent_rate); 54 } 55 56 static int clk_composite_determine_rate(struct clk_hw *hw, 57 struct clk_rate_request *req) 58 { 59 struct clk_composite *composite = to_clk_composite(hw); 60 const struct clk_ops *rate_ops = composite->rate_ops; 61 const struct clk_ops *mux_ops = composite->mux_ops; 62 struct clk_hw *rate_hw = composite->rate_hw; 63 struct clk_hw *mux_hw = composite->mux_hw; 64 struct clk_hw *parent; 65 unsigned long parent_rate; 66 long tmp_rate, best_rate = 0; 67 unsigned long rate_diff; 68 unsigned long best_rate_diff = ULONG_MAX; 69 long rate; 70 int i; 71 72 if (rate_hw && rate_ops && rate_ops->determine_rate) { 73 __clk_hw_set_clk(rate_hw, hw); 74 return rate_ops->determine_rate(rate_hw, req); 75 } else if (rate_hw && rate_ops && rate_ops->round_rate && 76 mux_hw && mux_ops && mux_ops->set_parent) { 77 req->best_parent_hw = NULL; 78 79 if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) { 80 parent = clk_hw_get_parent(mux_hw); 81 req->best_parent_hw = parent; 82 req->best_parent_rate = clk_hw_get_rate(parent); 83 84 rate = rate_ops->round_rate(rate_hw, req->rate, 85 &req->best_parent_rate); 86 if (rate < 0) 87 return rate; 88 89 req->rate = rate; 90 return 0; 91 } 92 93 for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) { 94 parent = clk_hw_get_parent_by_index(mux_hw, i); 95 if (!parent) 96 continue; 97 98 parent_rate = clk_hw_get_rate(parent); 99 100 tmp_rate = rate_ops->round_rate(rate_hw, req->rate, 101 &parent_rate); 102 if (tmp_rate < 0) 103 continue; 104 105 rate_diff = abs(req->rate - tmp_rate); 106 107 if (!rate_diff || !req->best_parent_hw 108 || best_rate_diff > rate_diff) { 109 req->best_parent_hw = parent; 110 req->best_parent_rate = parent_rate; 111 best_rate_diff = rate_diff; 112 best_rate = tmp_rate; 113 } 114 115 if (!rate_diff) 116 return 0; 117 } 118 119 req->rate = best_rate; 120 return 0; 121 } else if (mux_hw && mux_ops && mux_ops->determine_rate) { 122 __clk_hw_set_clk(mux_hw, hw); 123 return mux_ops->determine_rate(mux_hw, req); 124 } else { 125 pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n"); 126 return -EINVAL; 127 } 128 } 129 130 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate, 131 unsigned long *prate) 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->round_rate(rate_hw, rate, prate); 140 } 141 142 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate, 143 unsigned long parent_rate) 144 { 145 struct clk_composite *composite = to_clk_composite(hw); 146 const struct clk_ops *rate_ops = composite->rate_ops; 147 struct clk_hw *rate_hw = composite->rate_hw; 148 149 __clk_hw_set_clk(rate_hw, hw); 150 151 return rate_ops->set_rate(rate_hw, rate, parent_rate); 152 } 153 154 static int clk_composite_is_enabled(struct clk_hw *hw) 155 { 156 struct clk_composite *composite = to_clk_composite(hw); 157 const struct clk_ops *gate_ops = composite->gate_ops; 158 struct clk_hw *gate_hw = composite->gate_hw; 159 160 __clk_hw_set_clk(gate_hw, hw); 161 162 return gate_ops->is_enabled(gate_hw); 163 } 164 165 static int clk_composite_enable(struct clk_hw *hw) 166 { 167 struct clk_composite *composite = to_clk_composite(hw); 168 const struct clk_ops *gate_ops = composite->gate_ops; 169 struct clk_hw *gate_hw = composite->gate_hw; 170 171 __clk_hw_set_clk(gate_hw, hw); 172 173 return gate_ops->enable(gate_hw); 174 } 175 176 static void clk_composite_disable(struct clk_hw *hw) 177 { 178 struct clk_composite *composite = to_clk_composite(hw); 179 const struct clk_ops *gate_ops = composite->gate_ops; 180 struct clk_hw *gate_hw = composite->gate_hw; 181 182 __clk_hw_set_clk(gate_hw, hw); 183 184 gate_ops->disable(gate_hw); 185 } 186 187 struct clk *clk_register_composite(struct device *dev, const char *name, 188 const char * const *parent_names, int num_parents, 189 struct clk_hw *mux_hw, const struct clk_ops *mux_ops, 190 struct clk_hw *rate_hw, const struct clk_ops *rate_ops, 191 struct clk_hw *gate_hw, const struct clk_ops *gate_ops, 192 unsigned long flags) 193 { 194 struct clk *clk; 195 struct clk_init_data init; 196 struct clk_composite *composite; 197 struct clk_ops *clk_composite_ops; 198 199 composite = kzalloc(sizeof(*composite), GFP_KERNEL); 200 if (!composite) 201 return ERR_PTR(-ENOMEM); 202 203 init.name = name; 204 init.flags = flags | CLK_IS_BASIC; 205 init.parent_names = parent_names; 206 init.num_parents = num_parents; 207 208 clk_composite_ops = &composite->ops; 209 210 if (mux_hw && mux_ops) { 211 if (!mux_ops->get_parent) { 212 clk = ERR_PTR(-EINVAL); 213 goto err; 214 } 215 216 composite->mux_hw = mux_hw; 217 composite->mux_ops = mux_ops; 218 clk_composite_ops->get_parent = clk_composite_get_parent; 219 if (mux_ops->set_parent) 220 clk_composite_ops->set_parent = clk_composite_set_parent; 221 if (mux_ops->determine_rate) 222 clk_composite_ops->determine_rate = clk_composite_determine_rate; 223 } 224 225 if (rate_hw && rate_ops) { 226 if (!rate_ops->recalc_rate) { 227 clk = ERR_PTR(-EINVAL); 228 goto err; 229 } 230 clk_composite_ops->recalc_rate = clk_composite_recalc_rate; 231 232 if (rate_ops->determine_rate) 233 clk_composite_ops->determine_rate = 234 clk_composite_determine_rate; 235 else if (rate_ops->round_rate) 236 clk_composite_ops->round_rate = 237 clk_composite_round_rate; 238 239 /* .set_rate requires either .round_rate or .determine_rate */ 240 if (rate_ops->set_rate) { 241 if (rate_ops->determine_rate || rate_ops->round_rate) 242 clk_composite_ops->set_rate = 243 clk_composite_set_rate; 244 else 245 WARN(1, "%s: missing round_rate op is required\n", 246 __func__); 247 } 248 249 composite->rate_hw = rate_hw; 250 composite->rate_ops = rate_ops; 251 } 252 253 if (gate_hw && gate_ops) { 254 if (!gate_ops->is_enabled || !gate_ops->enable || 255 !gate_ops->disable) { 256 clk = ERR_PTR(-EINVAL); 257 goto err; 258 } 259 260 composite->gate_hw = gate_hw; 261 composite->gate_ops = gate_ops; 262 clk_composite_ops->is_enabled = clk_composite_is_enabled; 263 clk_composite_ops->enable = clk_composite_enable; 264 clk_composite_ops->disable = clk_composite_disable; 265 } 266 267 init.ops = clk_composite_ops; 268 composite->hw.init = &init; 269 270 clk = clk_register(dev, &composite->hw); 271 if (IS_ERR(clk)) 272 goto err; 273 274 if (composite->mux_hw) 275 composite->mux_hw->clk = clk; 276 277 if (composite->rate_hw) 278 composite->rate_hw->clk = clk; 279 280 if (composite->gate_hw) 281 composite->gate_hw->clk = clk; 282 283 return clk; 284 285 err: 286 kfree(composite); 287 return clk; 288 } 289