drivers/clk/clk-composite.c

0001 // SPDX-License-Identifier: GPL-2.0
0002 /*
0003  * Copyright (c) 2013 NVIDIA CORPORATION.  All rights reserved.
0004  */
0005
0006 #include <linux/clk-provider.h>
0007 #include <linux/device.h>
0008 #include <linux/err.h>
0009 #include <linux/slab.h>
0010
0011 static u8 clk_composite_get_parent(struct clk_hw *hw)
0012 {
0013     struct clk_composite *composite = to_clk_composite(hw);
0014     const struct clk_ops *mux_ops = composite->mux_ops;
0015     struct clk_hw *mux_hw = composite->mux_hw;
0016
0017     __clk_hw_set_clk(mux_hw, hw);
0018
0019     return mux_ops->get_parent(mux_hw);
0020 }
0021
0022 static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
0023 {
0024     struct clk_composite *composite = to_clk_composite(hw);
0025     const struct clk_ops *mux_ops = composite->mux_ops;
0026     struct clk_hw *mux_hw = composite->mux_hw;
0027
0028     __clk_hw_set_clk(mux_hw, hw);
0029
0030     return mux_ops->set_parent(mux_hw, index);
0031 }
0032
0033 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
0034                         unsigned long parent_rate)
0035 {
0036     struct clk_composite *composite = to_clk_composite(hw);
0037     const struct clk_ops *rate_ops = composite->rate_ops;
0038     struct clk_hw *rate_hw = composite->rate_hw;
0039
0040     __clk_hw_set_clk(rate_hw, hw);
0041
0042     return rate_ops->recalc_rate(rate_hw, parent_rate);
0043 }
0044
0045 static int clk_composite_determine_rate_for_parent(struct clk_hw *rate_hw,
0046                            struct clk_rate_request *req,
0047                            struct clk_hw *parent_hw,
0048                            const struct clk_ops *rate_ops)
0049 {
0050     long rate;
0051
0052     req->best_parent_hw = parent_hw;
0053     req->best_parent_rate = clk_hw_get_rate(parent_hw);
0054
0055     if (rate_ops->determine_rate)
0056         return rate_ops->determine_rate(rate_hw, req);
0057
0058     rate = rate_ops->round_rate(rate_hw, req->rate,
0059                     &req->best_parent_rate);
0060     if (rate < 0)
0061         return rate;
0062
0063     req->rate = rate;
0064
0065     return 0;
0066 }
0067
0068 static int clk_composite_determine_rate(struct clk_hw *hw,
0069                     struct clk_rate_request *req)
0070 {
0071     struct clk_composite *composite = to_clk_composite(hw);
0072     const struct clk_ops *rate_ops = composite->rate_ops;
0073     const struct clk_ops *mux_ops = composite->mux_ops;
0074     struct clk_hw *rate_hw = composite->rate_hw;
0075     struct clk_hw *mux_hw = composite->mux_hw;
0076     struct clk_hw *parent;
0077     unsigned long rate_diff;
0078     unsigned long best_rate_diff = ULONG_MAX;
0079     unsigned long best_rate = 0;
0080     int i, ret;
0081
0082     if (rate_hw && rate_ops &&
0083         (rate_ops->determine_rate || rate_ops->round_rate) &&
0084         mux_hw && mux_ops && mux_ops->set_parent) {
0085         req->best_parent_hw = NULL;
0086
0087         if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {
0088             struct clk_rate_request tmp_req = *req;
0089
0090             parent = clk_hw_get_parent(mux_hw);
0091
0092             ret = clk_composite_determine_rate_for_parent(rate_hw,
0093                                       &tmp_req,
0094                                       parent,
0095                                       rate_ops);
0096             if (ret)
0097                 return ret;
0098
0099             req->rate = tmp_req.rate;
0100             req->best_parent_hw = tmp_req.best_parent_hw;
0101             req->best_parent_rate = tmp_req.best_parent_rate;
0102
0103             return 0;
0104         }
0105
0106         for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) {
0107             struct clk_rate_request tmp_req = *req;
0108
0109             parent = clk_hw_get_parent_by_index(mux_hw, i);
0110             if (!parent)
0111                 continue;
0112
0113             ret = clk_composite_determine_rate_for_parent(rate_hw,
0114                                       &tmp_req,
0115                                       parent,
0116                                       rate_ops);
0117             if (ret)
0118                 continue;
0119
0120             rate_diff = abs(req->rate - tmp_req.rate);
0121
0122             if (!rate_diff || !req->best_parent_hw
0123                        || best_rate_diff > rate_diff) {
0124                 req->best_parent_hw = parent;
0125                 req->best_parent_rate = tmp_req.best_parent_rate;
0126                 best_rate_diff = rate_diff;
0127                 best_rate = tmp_req.rate;
0128             }
0129
0130             if (!rate_diff)
0131                 return 0;
0132         }
0133
0134         req->rate = best_rate;
0135         return 0;
0136     } else if (rate_hw && rate_ops && rate_ops->determine_rate) {
0137         __clk_hw_set_clk(rate_hw, hw);
0138         return rate_ops->determine_rate(rate_hw, req);
0139     } else if (mux_hw && mux_ops && mux_ops->determine_rate) {
0140         __clk_hw_set_clk(mux_hw, hw);
0141         return mux_ops->determine_rate(mux_hw, req);
0142     } else {
0143         pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
0144         return -EINVAL;
0145     }
0146 }
0147
0148 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
0149                   unsigned long *prate)
0150 {
0151     struct clk_composite *composite = to_clk_composite(hw);
0152     const struct clk_ops *rate_ops = composite->rate_ops;
0153     struct clk_hw *rate_hw = composite->rate_hw;
0154
0155     __clk_hw_set_clk(rate_hw, hw);
0156
0157     return rate_ops->round_rate(rate_hw, rate, prate);
0158 }
0159
0160 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
0161                    unsigned long parent_rate)
0162 {
0163     struct clk_composite *composite = to_clk_composite(hw);
0164     const struct clk_ops *rate_ops = composite->rate_ops;
0165     struct clk_hw *rate_hw = composite->rate_hw;
0166
0167     __clk_hw_set_clk(rate_hw, hw);
0168
0169     return rate_ops->set_rate(rate_hw, rate, parent_rate);
0170 }
0171
0172 static int clk_composite_set_rate_and_parent(struct clk_hw *hw,
0173                          unsigned long rate,
0174                          unsigned long parent_rate,
0175                          u8 index)
0176 {
0177     struct clk_composite *composite = to_clk_composite(hw);
0178     const struct clk_ops *rate_ops = composite->rate_ops;
0179     const struct clk_ops *mux_ops = composite->mux_ops;
0180     struct clk_hw *rate_hw = composite->rate_hw;
0181     struct clk_hw *mux_hw = composite->mux_hw;
0182     unsigned long temp_rate;
0183
0184     __clk_hw_set_clk(rate_hw, hw);
0185     __clk_hw_set_clk(mux_hw, hw);
0186
0187     temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate);
0188     if (temp_rate > rate) {
0189         rate_ops->set_rate(rate_hw, rate, parent_rate);
0190         mux_ops->set_parent(mux_hw, index);
0191     } else {
0192         mux_ops->set_parent(mux_hw, index);
0193         rate_ops->set_rate(rate_hw, rate, parent_rate);
0194     }
0195
0196     return 0;
0197 }
0198
0199 static int clk_composite_is_enabled(struct clk_hw *hw)
0200 {
0201     struct clk_composite *composite = to_clk_composite(hw);
0202     const struct clk_ops *gate_ops = composite->gate_ops;
0203     struct clk_hw *gate_hw = composite->gate_hw;
0204
0205     __clk_hw_set_clk(gate_hw, hw);
0206
0207     return gate_ops->is_enabled(gate_hw);
0208 }
0209
0210 static int clk_composite_enable(struct clk_hw *hw)
0211 {
0212     struct clk_composite *composite = to_clk_composite(hw);
0213     const struct clk_ops *gate_ops = composite->gate_ops;
0214     struct clk_hw *gate_hw = composite->gate_hw;
0215
0216     __clk_hw_set_clk(gate_hw, hw);
0217
0218     return gate_ops->enable(gate_hw);
0219 }
0220
0221 static void clk_composite_disable(struct clk_hw *hw)
0222 {
0223     struct clk_composite *composite = to_clk_composite(hw);
0224     const struct clk_ops *gate_ops = composite->gate_ops;
0225     struct clk_hw *gate_hw = composite->gate_hw;
0226
0227     __clk_hw_set_clk(gate_hw, hw);
0228
0229     gate_ops->disable(gate_hw);
0230 }
0231
0232 static struct clk_hw *__clk_hw_register_composite(struct device *dev,
0233             const char *name, const char * const *parent_names,
0234             const struct clk_parent_data *pdata, int num_parents,
0235             struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
0236             struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
0237             struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
0238             unsigned long flags)
0239 {
0240     struct clk_hw *hw;
0241     struct clk_init_data init = {};
0242     struct clk_composite *composite;
0243     struct clk_ops *clk_composite_ops;
0244     int ret;
0245
0246     composite = kzalloc(sizeof(*composite), GFP_KERNEL);
0247     if (!composite)
0248         return ERR_PTR(-ENOMEM);
0249
0250     init.name = name;
0251     init.flags = flags;
0252     if (parent_names)
0253         init.parent_names = parent_names;
0254     else
0255         init.parent_data = pdata;
0256     init.num_parents = num_parents;
0257     hw = &composite->hw;
0258
0259     clk_composite_ops = &composite->ops;
0260
0261     if (mux_hw && mux_ops) {
0262         if (!mux_ops->get_parent) {
0263             hw = ERR_PTR(-EINVAL);
0264             goto err;
0265         }
0266
0267         composite->mux_hw = mux_hw;
0268         composite->mux_ops = mux_ops;
0269         clk_composite_ops->get_parent = clk_composite_get_parent;
0270         if (mux_ops->set_parent)
0271             clk_composite_ops->set_parent = clk_composite_set_parent;
0272         if (mux_ops->determine_rate)
0273             clk_composite_ops->determine_rate = clk_composite_determine_rate;
0274     }
0275
0276     if (rate_hw && rate_ops) {
0277         if (!rate_ops->recalc_rate) {
0278             hw = ERR_PTR(-EINVAL);
0279             goto err;
0280         }
0281         clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
0282
0283         if (rate_ops->determine_rate)
0284             clk_composite_ops->determine_rate =
0285                 clk_composite_determine_rate;
0286         else if (rate_ops->round_rate)
0287             clk_composite_ops->round_rate =
0288                 clk_composite_round_rate;
0289
0290         /* .set_rate requires either .round_rate or .determine_rate */
0291         if (rate_ops->set_rate) {
0292             if (rate_ops->determine_rate || rate_ops->round_rate)
0293                 clk_composite_ops->set_rate =
0294                         clk_composite_set_rate;
0295             else
0296                 WARN(1, "%s: missing round_rate op is required\n",
0297                         __func__);
0298         }
0299
0300         composite->rate_hw = rate_hw;
0301         composite->rate_ops = rate_ops;
0302     }
0303
0304     if (mux_hw && mux_ops && rate_hw && rate_ops) {
0305         if (mux_ops->set_parent && rate_ops->set_rate)
0306             clk_composite_ops->set_rate_and_parent =
0307             clk_composite_set_rate_and_parent;
0308     }
0309
0310     if (gate_hw && gate_ops) {
0311         if (!gate_ops->is_enabled || !gate_ops->enable ||
0312             !gate_ops->disable) {
0313             hw = ERR_PTR(-EINVAL);
0314             goto err;
0315         }
0316
0317         composite->gate_hw = gate_hw;
0318         composite->gate_ops = gate_ops;
0319         clk_composite_ops->is_enabled = clk_composite_is_enabled;
0320         clk_composite_ops->enable = clk_composite_enable;
0321         clk_composite_ops->disable = clk_composite_disable;
0322     }
0323
0324     init.ops = clk_composite_ops;
0325     composite->hw.init = &init;
0326
0327     ret = clk_hw_register(dev, hw);
0328     if (ret) {
0329         hw = ERR_PTR(ret);
0330         goto err;
0331     }
0332
0333     if (composite->mux_hw)
0334         composite->mux_hw->clk = hw->clk;
0335
0336     if (composite->rate_hw)
0337         composite->rate_hw->clk = hw->clk;
0338
0339     if (composite->gate_hw)
0340         composite->gate_hw->clk = hw->clk;
0341
0342     return hw;
0343
0344 err:
0345     kfree(composite);
0346     return hw;
0347 }
0348
0349 struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name,
0350             const char * const *parent_names, int num_parents,
0351             struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
0352             struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
0353             struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
0354             unsigned long flags)
0355 {
0356     return __clk_hw_register_composite(dev, name, parent_names, NULL,
0357                        num_parents, mux_hw, mux_ops,
0358                        rate_hw, rate_ops, gate_hw,
0359                        gate_ops, flags);
0360 }
0361 EXPORT_SYMBOL_GPL(clk_hw_register_composite);
0362
0363 struct clk_hw *clk_hw_register_composite_pdata(struct device *dev,
0364             const char *name,
0365             const struct clk_parent_data *parent_data,
0366             int num_parents,
0367             struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
0368             struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
0369             struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
0370             unsigned long flags)
0371 {
0372     return __clk_hw_register_composite(dev, name, NULL, parent_data,
0373                        num_parents, mux_hw, mux_ops,
0374                        rate_hw, rate_ops, gate_hw,
0375                        gate_ops, flags);
0376 }
0377
0378 struct clk *clk_register_composite(struct device *dev, const char *name,
0379             const char * const *parent_names, int num_parents,
0380             struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
0381             struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
0382             struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
0383             unsigned long flags)
0384 {
0385     struct clk_hw *hw;
0386
0387     hw = clk_hw_register_composite(dev, name, parent_names, num_parents,
0388             mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops,
0389             flags);
0390     if (IS_ERR(hw))
0391         return ERR_CAST(hw);
0392     return hw->clk;
0393 }
0394 EXPORT_SYMBOL_GPL(clk_register_composite);
0395
0396 struct clk *clk_register_composite_pdata(struct device *dev, const char *name,
0397             const struct clk_parent_data *parent_data,
0398             int num_parents,
0399             struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
0400             struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
0401             struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
0402             unsigned long flags)
0403 {
0404     struct clk_hw *hw;
0405
0406     hw = clk_hw_register_composite_pdata(dev, name, parent_data,
0407             num_parents, mux_hw, mux_ops, rate_hw, rate_ops,
0408             gate_hw, gate_ops, flags);
0409     if (IS_ERR(hw))
0410         return ERR_CAST(hw);
0411     return hw->clk;
0412 }
0413
0414 void clk_unregister_composite(struct clk *clk)
0415 {
0416     struct clk_composite *composite;
0417     struct clk_hw *hw;
0418
0419     hw = __clk_get_hw(clk);
0420     if (!hw)
0421         return;
0422
0423     composite = to_clk_composite(hw);
0424
0425     clk_unregister(clk);
0426     kfree(composite);
0427 }
0428
0429 void clk_hw_unregister_composite(struct clk_hw *hw)
0430 {
0431     struct clk_composite *composite;
0432
0433     composite = to_clk_composite(hw);
0434
0435     clk_hw_unregister(hw);
0436     kfree(composite);
0437 }
0438 EXPORT_SYMBOL_GPL(clk_hw_unregister_composite);
0439
0440 static void devm_clk_hw_release_composite(struct device *dev, void *res)
0441 {
0442     clk_hw_unregister_composite(*(struct clk_hw **)res);
0443 }
0444
0445 static struct clk_hw *__devm_clk_hw_register_composite(struct device *dev,
0446             const char *name, const char * const *parent_names,
0447             const struct clk_parent_data *pdata, int num_parents,
0448             struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
0449             struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
0450             struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
0451             unsigned long flags)
0452 {
0453     struct clk_hw **ptr, *hw;
0454
0455     ptr = devres_alloc(devm_clk_hw_release_composite, sizeof(*ptr),
0456                GFP_KERNEL);
0457     if (!ptr)
0458         return ERR_PTR(-ENOMEM);
0459
0460     hw = __clk_hw_register_composite(dev, name, parent_names, pdata,
0461                      num_parents, mux_hw, mux_ops, rate_hw,
0462                      rate_ops, gate_hw, gate_ops, flags);
0463
0464     if (!IS_ERR(hw)) {
0465         *ptr = hw;
0466         devres_add(dev, ptr);
0467     } else {
0468         devres_free(ptr);
0469     }
0470
0471     return hw;
0472 }
0473
0474 struct clk_hw *devm_clk_hw_register_composite_pdata(struct device *dev,
0475             const char *name,
0476             const struct clk_parent_data *parent_data,
0477             int num_parents,
0478             struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
0479             struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
0480             struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
0481             unsigned long flags)
0482 {
0483     return __devm_clk_hw_register_composite(dev, name, NULL, parent_data,
0484                         num_parents, mux_hw, mux_ops,
0485                         rate_hw, rate_ops, gate_hw,
0486                         gate_ops, flags);
0487 }