drivers/clk/clk-composite.c

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