drivers/clk/clk-composite.c

   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/device.h>
   8 #include <linux/err.h>
   9 #include <linux/slab.h>
  10
  11 static u8 clk_composite_get_parent(struct clk_hw *hw)
  12 {
  13         struct clk_composite *composite = to_clk_composite(hw);
  14         const struct clk_ops *mux_ops = composite->mux_ops;
  15         struct clk_hw *mux_hw = composite->mux_hw;
  16
  17         __clk_hw_set_clk(mux_hw, hw);
  18
  19         return mux_ops->get_parent(mux_hw);
  20 }
  21
  22 static int clk_composite_set_parent(struct clk_hw *hw, u8 index)
  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->set_parent(mux_hw, index);
  31 }
  32
  33 static unsigned long clk_composite_recalc_rate(struct clk_hw *hw,
  34                                             unsigned long parent_rate)
  35 {
  36         struct clk_composite *composite = to_clk_composite(hw);
  37         const struct clk_ops *rate_ops = composite->rate_ops;
  38         struct clk_hw *rate_hw = composite->rate_hw;
  39
  40         __clk_hw_set_clk(rate_hw, hw);
  41
  42         return rate_ops->recalc_rate(rate_hw, parent_rate);
  43 }
  44
  45 static int clk_composite_determine_rate_for_parent(struct clk_hw *rate_hw,
  46                                                    struct clk_rate_request *req,
  47                                                    struct clk_hw *parent_hw,
  48                                                    const struct clk_ops *rate_ops)
  49 {
  50         long rate;
  51
  52         req->best_parent_hw = parent_hw;
  53         req->best_parent_rate = clk_hw_get_rate(parent_hw);
  54
  55         if (rate_ops->determine_rate)
  56                 return rate_ops->determine_rate(rate_hw, req);
  57
  58         rate = rate_ops->round_rate(rate_hw, req->rate,
  59                                     &req->best_parent_rate);
  60         if (rate < 0)
  61                 return rate;
  62
  63         req->rate = rate;
  64
  65         return 0;
  66 }
  67
  68 static int clk_composite_determine_rate(struct clk_hw *hw,
  69                                         struct clk_rate_request *req)
  70 {
  71         struct clk_composite *composite = to_clk_composite(hw);
  72         const struct clk_ops *rate_ops = composite->rate_ops;
  73         const struct clk_ops *mux_ops = composite->mux_ops;
  74         struct clk_hw *rate_hw = composite->rate_hw;
  75         struct clk_hw *mux_hw = composite->mux_hw;
  76         struct clk_hw *parent;
  77         unsigned long rate_diff;
  78         unsigned long best_rate_diff = ULONG_MAX;
  79         unsigned long best_rate = 0;
  80         int i, ret;
  81
  82         if (rate_hw && rate_ops &&
  83             (rate_ops->determine_rate || rate_ops->round_rate) &&
  84             mux_hw && mux_ops && mux_ops->set_parent) {
  85                 req->best_parent_hw = NULL;
  86
  87                 if (clk_hw_get_flags(hw) & CLK_SET_RATE_NO_REPARENT) {
  88                         struct clk_rate_request tmp_req;
  89
  90                         parent = clk_hw_get_parent(mux_hw);
  91
  92                         clk_hw_forward_rate_request(hw, req, parent, &tmp_req, req->rate);
  93                         ret = clk_composite_determine_rate_for_parent(rate_hw,
  94                                                                       &tmp_req,
  95                                                                       parent,
  96                                                                       rate_ops);
  97                         if (ret)
  98                                 return ret;
  99
 100                         req->rate = tmp_req.rate;
 101                         req->best_parent_hw = tmp_req.best_parent_hw;
 102                         req->best_parent_rate = tmp_req.best_parent_rate;
 103
 104                         return 0;
 105                 }
 106
 107                 for (i = 0; i < clk_hw_get_num_parents(mux_hw); i++) {
 108                         struct clk_rate_request tmp_req;
 109
 110                         parent = clk_hw_get_parent_by_index(mux_hw, i);
 111                         if (!parent)
 112                                 continue;
 113
 114                         clk_hw_forward_rate_request(hw, req, parent, &tmp_req, req->rate);
 115                         ret = clk_composite_determine_rate_for_parent(rate_hw,
 116                                                                       &tmp_req,
 117                                                                       parent,
 118                                                                       rate_ops);
 119                         if (ret)
 120                                 continue;
 121
 122                         if (req->rate >= tmp_req.rate)
 123                                 rate_diff = req->rate - tmp_req.rate;
 124                         else
 125                                 rate_diff = tmp_req.rate - req->rate;
 126
 127                         if (!rate_diff || !req->best_parent_hw
 128                                        || best_rate_diff > rate_diff) {
 129                                 req->best_parent_hw = parent;
 130                                 req->best_parent_rate = tmp_req.best_parent_rate;
 131                                 best_rate_diff = rate_diff;
 132                                 best_rate = tmp_req.rate;
 133                         }
 134
 135                         if (!rate_diff)
 136                                 return 0;
 137                 }
 138
 139                 req->rate = best_rate;
 140                 return 0;
 141         } else if (rate_hw && rate_ops && rate_ops->determine_rate) {
 142                 __clk_hw_set_clk(rate_hw, hw);
 143                 return rate_ops->determine_rate(rate_hw, req);
 144         } else if (mux_hw && mux_ops && mux_ops->determine_rate) {
 145                 __clk_hw_set_clk(mux_hw, hw);
 146                 return mux_ops->determine_rate(mux_hw, req);
 147         } else {
 148                 pr_err("clk: clk_composite_determine_rate function called, but no mux or rate callback set!\n");
 149                 return -EINVAL;
 150         }
 151 }
 152
 153 static long clk_composite_round_rate(struct clk_hw *hw, unsigned long rate,
 154                                   unsigned long *prate)
 155 {
 156         struct clk_composite *composite = to_clk_composite(hw);
 157         const struct clk_ops *rate_ops = composite->rate_ops;
 158         struct clk_hw *rate_hw = composite->rate_hw;
 159
 160         __clk_hw_set_clk(rate_hw, hw);
 161
 162         return rate_ops->round_rate(rate_hw, rate, prate);
 163 }
 164
 165 static int clk_composite_set_rate(struct clk_hw *hw, unsigned long rate,
 166                                unsigned long parent_rate)
 167 {
 168         struct clk_composite *composite = to_clk_composite(hw);
 169         const struct clk_ops *rate_ops = composite->rate_ops;
 170         struct clk_hw *rate_hw = composite->rate_hw;
 171
 172         __clk_hw_set_clk(rate_hw, hw);
 173
 174         return rate_ops->set_rate(rate_hw, rate, parent_rate);
 175 }
 176
 177 static int clk_composite_set_rate_and_parent(struct clk_hw *hw,
 178                                              unsigned long rate,
 179                                              unsigned long parent_rate,
 180                                              u8 index)
 181 {
 182         struct clk_composite *composite = to_clk_composite(hw);
 183         const struct clk_ops *rate_ops = composite->rate_ops;
 184         const struct clk_ops *mux_ops = composite->mux_ops;
 185         struct clk_hw *rate_hw = composite->rate_hw;
 186         struct clk_hw *mux_hw = composite->mux_hw;
 187         unsigned long temp_rate;
 188
 189         __clk_hw_set_clk(rate_hw, hw);
 190         __clk_hw_set_clk(mux_hw, hw);
 191
 192         temp_rate = rate_ops->recalc_rate(rate_hw, parent_rate);
 193         if (temp_rate > rate) {
 194                 rate_ops->set_rate(rate_hw, rate, parent_rate);
 195                 mux_ops->set_parent(mux_hw, index);
 196         } else {
 197                 mux_ops->set_parent(mux_hw, index);
 198                 rate_ops->set_rate(rate_hw, rate, parent_rate);
 199         }
 200
 201         return 0;
 202 }
 203
 204 static int clk_composite_is_enabled(struct clk_hw *hw)
 205 {
 206         struct clk_composite *composite = to_clk_composite(hw);
 207         const struct clk_ops *gate_ops = composite->gate_ops;
 208         struct clk_hw *gate_hw = composite->gate_hw;
 209
 210         __clk_hw_set_clk(gate_hw, hw);
 211
 212         return gate_ops->is_enabled(gate_hw);
 213 }
 214
 215 static int clk_composite_enable(struct clk_hw *hw)
 216 {
 217         struct clk_composite *composite = to_clk_composite(hw);
 218         const struct clk_ops *gate_ops = composite->gate_ops;
 219         struct clk_hw *gate_hw = composite->gate_hw;
 220
 221         __clk_hw_set_clk(gate_hw, hw);
 222
 223         return gate_ops->enable(gate_hw);
 224 }
 225
 226 static void clk_composite_disable(struct clk_hw *hw)
 227 {
 228         struct clk_composite *composite = to_clk_composite(hw);
 229         const struct clk_ops *gate_ops = composite->gate_ops;
 230         struct clk_hw *gate_hw = composite->gate_hw;
 231
 232         __clk_hw_set_clk(gate_hw, hw);
 233
 234         gate_ops->disable(gate_hw);
 235 }
 236
 237 static struct clk_hw *__clk_hw_register_composite(struct device *dev,
 238                         const char *name, const char * const *parent_names,
 239                         const struct clk_parent_data *pdata, int num_parents,
 240                         struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
 241                         struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
 242                         struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
 243                         unsigned long flags)
 244 {
 245         struct clk_hw *hw;
 246         struct clk_init_data init = {};
 247         struct clk_composite *composite;
 248         struct clk_ops *clk_composite_ops;
 249         int ret;
 250
 251         composite = kzalloc(sizeof(*composite), GFP_KERNEL);
 252         if (!composite)
 253                 return ERR_PTR(-ENOMEM);
 254
 255         init.name = name;
 256         init.flags = flags;
 257         if (parent_names)
 258                 init.parent_names = parent_names;
 259         else
 260                 init.parent_data = pdata;
 261         init.num_parents = num_parents;
 262         hw = &composite->hw;
 263
 264         clk_composite_ops = &composite->ops;
 265
 266         if (mux_hw && mux_ops) {
 267                 if (!mux_ops->get_parent) {
 268                         hw = ERR_PTR(-EINVAL);
 269                         goto err;
 270                 }
 271
 272                 composite->mux_hw = mux_hw;
 273                 composite->mux_ops = mux_ops;
 274                 clk_composite_ops->get_parent = clk_composite_get_parent;
 275                 if (mux_ops->set_parent)
 276                         clk_composite_ops->set_parent = clk_composite_set_parent;
 277                 if (mux_ops->determine_rate)
 278                         clk_composite_ops->determine_rate = clk_composite_determine_rate;
 279         }
 280
 281         if (rate_hw && rate_ops) {
 282                 if (!rate_ops->recalc_rate) {
 283                         hw = ERR_PTR(-EINVAL);
 284                         goto err;
 285                 }
 286                 clk_composite_ops->recalc_rate = clk_composite_recalc_rate;
 287
 288                 if (rate_ops->determine_rate)
 289                         clk_composite_ops->determine_rate =
 290                                 clk_composite_determine_rate;
 291                 else if (rate_ops->round_rate)
 292                         clk_composite_ops->round_rate =
 293                                 clk_composite_round_rate;
 294
 295                 /* .set_rate requires either .round_rate or .determine_rate */
 296                 if (rate_ops->set_rate) {
 297                         if (rate_ops->determine_rate || rate_ops->round_rate)
 298                                 clk_composite_ops->set_rate =
 299                                                 clk_composite_set_rate;
 300                         else
 301                                 WARN(1, "%s: missing round_rate op is required\n",
 302                                                 __func__);
 303                 }
 304
 305                 composite->rate_hw = rate_hw;
 306                 composite->rate_ops = rate_ops;
 307         }
 308
 309         if (mux_hw && mux_ops && rate_hw && rate_ops) {
 310                 if (mux_ops->set_parent && rate_ops->set_rate)
 311                         clk_composite_ops->set_rate_and_parent =
 312                         clk_composite_set_rate_and_parent;
 313         }
 314
 315         if (gate_hw && gate_ops) {
 316                 if (!gate_ops->is_enabled || !gate_ops->enable ||
 317                     !gate_ops->disable) {
 318                         hw = ERR_PTR(-EINVAL);
 319                         goto err;
 320                 }
 321
 322                 composite->gate_hw = gate_hw;
 323                 composite->gate_ops = gate_ops;
 324                 clk_composite_ops->is_enabled = clk_composite_is_enabled;
 325                 clk_composite_ops->enable = clk_composite_enable;
 326                 clk_composite_ops->disable = clk_composite_disable;
 327         }
 328
 329         init.ops = clk_composite_ops;
 330         composite->hw.init = &init;
 331
 332         ret = clk_hw_register(dev, hw);
 333         if (ret) {
 334                 hw = ERR_PTR(ret);
 335                 goto err;
 336         }
 337
 338         if (composite->mux_hw)
 339                 composite->mux_hw->clk = hw->clk;
 340
 341         if (composite->rate_hw)
 342                 composite->rate_hw->clk = hw->clk;
 343
 344         if (composite->gate_hw)
 345                 composite->gate_hw->clk = hw->clk;
 346
 347         return hw;
 348
 349 err:
 350         kfree(composite);
 351         return hw;
 352 }
 353
 354 struct clk_hw *clk_hw_register_composite(struct device *dev, const char *name,
 355                         const char * const *parent_names, int num_parents,
 356                         struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
 357                         struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
 358                         struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
 359                         unsigned long flags)
 360 {
 361         return __clk_hw_register_composite(dev, name, parent_names, NULL,
 362                                            num_parents, mux_hw, mux_ops,
 363                                            rate_hw, rate_ops, gate_hw,
 364                                            gate_ops, flags);
 365 }
 366 EXPORT_SYMBOL_GPL(clk_hw_register_composite);
 367
 368 struct clk_hw *clk_hw_register_composite_pdata(struct device *dev,
 369                         const char *name,
 370                         const struct clk_parent_data *parent_data,
 371                         int num_parents,
 372                         struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
 373                         struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
 374                         struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
 375                         unsigned long flags)
 376 {
 377         return __clk_hw_register_composite(dev, name, NULL, parent_data,
 378                                            num_parents, mux_hw, mux_ops,
 379                                            rate_hw, rate_ops, gate_hw,
 380                                            gate_ops, flags);
 381 }
 382
 383 struct clk *clk_register_composite(struct device *dev, const char *name,
 384                         const char * const *parent_names, int num_parents,
 385                         struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
 386                         struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
 387                         struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
 388                         unsigned long flags)
 389 {
 390         struct clk_hw *hw;
 391
 392         hw = clk_hw_register_composite(dev, name, parent_names, num_parents,
 393                         mux_hw, mux_ops, rate_hw, rate_ops, gate_hw, gate_ops,
 394                         flags);
 395         if (IS_ERR(hw))
 396                 return ERR_CAST(hw);
 397         return hw->clk;
 398 }
 399 EXPORT_SYMBOL_GPL(clk_register_composite);
 400
 401 struct clk *clk_register_composite_pdata(struct device *dev, const char *name,
 402                         const struct clk_parent_data *parent_data,
 403                         int num_parents,
 404                         struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
 405                         struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
 406                         struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
 407                         unsigned long flags)
 408 {
 409         struct clk_hw *hw;
 410
 411         hw = clk_hw_register_composite_pdata(dev, name, parent_data,
 412                         num_parents, mux_hw, mux_ops, rate_hw, rate_ops,
 413                         gate_hw, gate_ops, flags);
 414         if (IS_ERR(hw))
 415                 return ERR_CAST(hw);
 416         return hw->clk;
 417 }
 418
 419 void clk_unregister_composite(struct clk *clk)
 420 {
 421         struct clk_composite *composite;
 422         struct clk_hw *hw;
 423
 424         hw = __clk_get_hw(clk);
 425         if (!hw)
 426                 return;
 427
 428         composite = to_clk_composite(hw);
 429
 430         clk_unregister(clk);
 431         kfree(composite);
 432 }
 433
 434 void clk_hw_unregister_composite(struct clk_hw *hw)
 435 {
 436         struct clk_composite *composite;
 437
 438         composite = to_clk_composite(hw);
 439
 440         clk_hw_unregister(hw);
 441         kfree(composite);
 442 }
 443 EXPORT_SYMBOL_GPL(clk_hw_unregister_composite);
 444
 445 static void devm_clk_hw_release_composite(struct device *dev, void *res)
 446 {
 447         clk_hw_unregister_composite(*(struct clk_hw **)res);
 448 }
 449
 450 static struct clk_hw *__devm_clk_hw_register_composite(struct device *dev,
 451                         const char *name, const char * const *parent_names,
 452                         const struct clk_parent_data *pdata, int num_parents,
 453                         struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
 454                         struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
 455                         struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
 456                         unsigned long flags)
 457 {
 458         struct clk_hw **ptr, *hw;
 459
 460         ptr = devres_alloc(devm_clk_hw_release_composite, sizeof(*ptr),
 461                            GFP_KERNEL);
 462         if (!ptr)
 463                 return ERR_PTR(-ENOMEM);
 464
 465         hw = __clk_hw_register_composite(dev, name, parent_names, pdata,
 466                                          num_parents, mux_hw, mux_ops, rate_hw,
 467                                          rate_ops, gate_hw, gate_ops, flags);
 468
 469         if (!IS_ERR(hw)) {
 470                 *ptr = hw;
 471                 devres_add(dev, ptr);
 472         } else {
 473                 devres_free(ptr);
 474         }
 475
 476         return hw;
 477 }
 478
 479 struct clk_hw *devm_clk_hw_register_composite_pdata(struct device *dev,
 480                         const char *name,
 481                         const struct clk_parent_data *parent_data,
 482                         int num_parents,
 483                         struct clk_hw *mux_hw, const struct clk_ops *mux_ops,
 484                         struct clk_hw *rate_hw, const struct clk_ops *rate_ops,
 485                         struct clk_hw *gate_hw, const struct clk_ops *gate_ops,
 486                         unsigned long flags)
 487 {
 488         return __devm_clk_hw_register_composite(dev, name, NULL, parent_data,
 489                                                 num_parents, mux_hw, mux_ops,
 490                                                 rate_hw, rate_ops, gate_hw,
 491                                                 gate_ops, flags);
 492 }