drivers/clk/at91/clk-peripheral.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *  Copyright (C) 2013 Boris BREZILLON <b.brezillon@overkiz.com>
   4  */
   5
   6 #include <linux/bitops.h>
   7 #include <linux/clk-provider.h>
   8 #include <linux/clkdev.h>
   9 #include <linux/clk/at91_pmc.h>
  10 #include <linux/of.h>
  11 #include <linux/mfd/syscon.h>
  12 #include <linux/regmap.h>
  13
  14 #include "pmc.h"
  15
  16 DEFINE_SPINLOCK(pmc_pcr_lock);
  17
  18 #define PERIPHERAL_ID_MIN       2
  19 #define PERIPHERAL_ID_MAX       31
  20 #define PERIPHERAL_MASK(id)     (1 << ((id) & PERIPHERAL_ID_MAX))
  21
  22 #define PERIPHERAL_MAX_SHIFT    3
  23
  24 struct clk_peripheral {
  25         struct clk_hw hw;
  26         struct regmap *regmap;
  27         u32 id;
  28 };
  29
  30 #define to_clk_peripheral(hw) container_of(hw, struct clk_peripheral, hw)
  31
  32 struct clk_sam9x5_peripheral {
  33         struct clk_hw hw;
  34         struct regmap *regmap;
  35         struct clk_range range;
  36         spinlock_t *lock;
  37         u32 id;
  38         u32 div;
  39         const struct clk_pcr_layout *layout;
  40         struct at91_clk_pms pms;
  41         bool auto_div;
  42         int chg_pid;
  43 };
  44
  45 #define to_clk_sam9x5_peripheral(hw) \
  46         container_of(hw, struct clk_sam9x5_peripheral, hw)
  47
  48 static int clk_peripheral_enable(struct clk_hw *hw)
  49 {
  50         struct clk_peripheral *periph = to_clk_peripheral(hw);
  51         int offset = AT91_PMC_PCER;
  52         u32 id = periph->id;
  53
  54         if (id < PERIPHERAL_ID_MIN)
  55                 return 0;
  56         if (id > PERIPHERAL_ID_MAX)
  57                 offset = AT91_PMC_PCER1;
  58         regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id));
  59
  60         return 0;
  61 }
  62
  63 static void clk_peripheral_disable(struct clk_hw *hw)
  64 {
  65         struct clk_peripheral *periph = to_clk_peripheral(hw);
  66         int offset = AT91_PMC_PCDR;
  67         u32 id = periph->id;
  68
  69         if (id < PERIPHERAL_ID_MIN)
  70                 return;
  71         if (id > PERIPHERAL_ID_MAX)
  72                 offset = AT91_PMC_PCDR1;
  73         regmap_write(periph->regmap, offset, PERIPHERAL_MASK(id));
  74 }
  75
  76 static int clk_peripheral_is_enabled(struct clk_hw *hw)
  77 {
  78         struct clk_peripheral *periph = to_clk_peripheral(hw);
  79         int offset = AT91_PMC_PCSR;
  80         unsigned int status;
  81         u32 id = periph->id;
  82
  83         if (id < PERIPHERAL_ID_MIN)
  84                 return 1;
  85         if (id > PERIPHERAL_ID_MAX)
  86                 offset = AT91_PMC_PCSR1;
  87         regmap_read(periph->regmap, offset, &status);
  88
  89         return status & PERIPHERAL_MASK(id) ? 1 : 0;
  90 }
  91
  92 static const struct clk_ops peripheral_ops = {
  93         .enable = clk_peripheral_enable,
  94         .disable = clk_peripheral_disable,
  95         .is_enabled = clk_peripheral_is_enabled,
  96 };
  97
  98 struct clk_hw * __init
  99 at91_clk_register_peripheral(struct regmap *regmap, const char *name,
 100                              const char *parent_name, struct clk_hw *parent_hw,
 101                              u32 id)
 102 {
 103         struct clk_peripheral *periph;
 104         struct clk_init_data init = {};
 105         struct clk_hw *hw;
 106         int ret;
 107
 108         if (!name || !(parent_name || parent_hw) || id > PERIPHERAL_ID_MAX)
 109                 return ERR_PTR(-EINVAL);
 110
 111         periph = kzalloc(sizeof(*periph), GFP_KERNEL);
 112         if (!periph)
 113                 return ERR_PTR(-ENOMEM);
 114
 115         init.name = name;
 116         init.ops = &peripheral_ops;
 117         if (parent_hw)
 118                 init.parent_hws = (const struct clk_hw **)&parent_hw;
 119         else
 120                 init.parent_names = &parent_name;
 121         init.num_parents = 1;
 122         init.flags = 0;
 123
 124         periph->id = id;
 125         periph->hw.init = &init;
 126         periph->regmap = regmap;
 127
 128         hw = &periph->hw;
 129         ret = clk_hw_register(NULL, &periph->hw);
 130         if (ret) {
 131                 kfree(periph);
 132                 hw = ERR_PTR(ret);
 133         }
 134
 135         return hw;
 136 }
 137
 138 static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph)
 139 {
 140         struct clk_hw *parent;
 141         unsigned long parent_rate;
 142         int shift = 0;
 143
 144         if (!periph->auto_div)
 145                 return;
 146
 147         if (periph->range.max) {
 148                 parent = clk_hw_get_parent_by_index(&periph->hw, 0);
 149                 parent_rate = clk_hw_get_rate(parent);
 150                 if (!parent_rate)
 151                         return;
 152
 153                 for (; shift < PERIPHERAL_MAX_SHIFT; shift++) {
 154                         if (parent_rate >> shift <= periph->range.max)
 155                                 break;
 156                 }
 157         }
 158
 159         periph->auto_div = false;
 160         periph->div = shift;
 161 }
 162
 163 static int clk_sam9x5_peripheral_set(struct clk_sam9x5_peripheral *periph,
 164                                      unsigned int status)
 165 {
 166         unsigned long flags;
 167         unsigned int enable = status ? AT91_PMC_PCR_EN : 0;
 168
 169         if (periph->id < PERIPHERAL_ID_MIN)
 170                 return 0;
 171
 172         spin_lock_irqsave(periph->lock, flags);
 173         regmap_write(periph->regmap, periph->layout->offset,
 174                      (periph->id & periph->layout->pid_mask));
 175         regmap_update_bits(periph->regmap, periph->layout->offset,
 176                            periph->layout->div_mask | periph->layout->cmd |
 177                            enable,
 178                            field_prep(periph->layout->div_mask, periph->div) |
 179                            periph->layout->cmd | enable);
 180         spin_unlock_irqrestore(periph->lock, flags);
 181
 182         return 0;
 183 }
 184
 185 static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
 186 {
 187         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 188
 189         return clk_sam9x5_peripheral_set(periph, 1);
 190 }
 191
 192 static void clk_sam9x5_peripheral_disable(struct clk_hw *hw)
 193 {
 194         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 195         unsigned long flags;
 196
 197         if (periph->id < PERIPHERAL_ID_MIN)
 198                 return;
 199
 200         spin_lock_irqsave(periph->lock, flags);
 201         regmap_write(periph->regmap, periph->layout->offset,
 202                      (periph->id & periph->layout->pid_mask));
 203         regmap_update_bits(periph->regmap, periph->layout->offset,
 204                            AT91_PMC_PCR_EN | periph->layout->cmd,
 205                            periph->layout->cmd);
 206         spin_unlock_irqrestore(periph->lock, flags);
 207 }
 208
 209 static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw)
 210 {
 211         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 212         unsigned long flags;
 213         unsigned int status;
 214
 215         if (periph->id < PERIPHERAL_ID_MIN)
 216                 return 1;
 217
 218         spin_lock_irqsave(periph->lock, flags);
 219         regmap_write(periph->regmap, periph->layout->offset,
 220                      (periph->id & periph->layout->pid_mask));
 221         regmap_read(periph->regmap, periph->layout->offset, &status);
 222         spin_unlock_irqrestore(periph->lock, flags);
 223
 224         return !!(status & AT91_PMC_PCR_EN);
 225 }
 226
 227 static unsigned long
 228 clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw,
 229                                   unsigned long parent_rate)
 230 {
 231         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 232         unsigned long flags;
 233         unsigned int status;
 234
 235         if (periph->id < PERIPHERAL_ID_MIN)
 236                 return parent_rate;
 237
 238         spin_lock_irqsave(periph->lock, flags);
 239         regmap_write(periph->regmap, periph->layout->offset,
 240                      (periph->id & periph->layout->pid_mask));
 241         regmap_read(periph->regmap, periph->layout->offset, &status);
 242         spin_unlock_irqrestore(periph->lock, flags);
 243
 244         if (status & AT91_PMC_PCR_EN) {
 245                 periph->div = field_get(periph->layout->div_mask, status);
 246                 periph->auto_div = false;
 247         } else {
 248                 clk_sam9x5_peripheral_autodiv(periph);
 249         }
 250
 251         return parent_rate >> periph->div;
 252 }
 253
 254 static void clk_sam9x5_peripheral_best_diff(struct clk_rate_request *req,
 255                                             struct clk_hw *parent,
 256                                             unsigned long parent_rate,
 257                                             u32 shift, long *best_diff,
 258                                             long *best_rate)
 259 {
 260         unsigned long tmp_rate = parent_rate >> shift;
 261         unsigned long tmp_diff = abs(req->rate - tmp_rate);
 262
 263         if (*best_diff < 0 || *best_diff >= tmp_diff) {
 264                 *best_rate = tmp_rate;
 265                 *best_diff = tmp_diff;
 266                 req->best_parent_rate = parent_rate;
 267                 req->best_parent_hw = parent;
 268         }
 269 }
 270
 271 static int clk_sam9x5_peripheral_determine_rate(struct clk_hw *hw,
 272                                                 struct clk_rate_request *req)
 273 {
 274         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 275         struct clk_hw *parent = clk_hw_get_parent(hw);
 276         unsigned long parent_rate = clk_hw_get_rate(parent);
 277         unsigned long tmp_rate;
 278         long best_rate = LONG_MIN;
 279         long best_diff = LONG_MIN;
 280         u32 shift;
 281
 282         if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max)
 283                 return parent_rate;
 284
 285         /* Fist step: check the available dividers. */
 286         for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 287                 tmp_rate = parent_rate >> shift;
 288
 289                 if (periph->range.max && tmp_rate > periph->range.max)
 290                         continue;
 291
 292                 clk_sam9x5_peripheral_best_diff(req, parent, parent_rate,
 293                                                 shift, &best_diff, &best_rate);
 294
 295                 if (!best_diff || best_rate <= req->rate)
 296                         break;
 297         }
 298
 299         if (periph->chg_pid < 0)
 300                 goto end;
 301
 302         /* Step two: try to request rate from parent. */
 303         parent = clk_hw_get_parent_by_index(hw, periph->chg_pid);
 304         if (!parent)
 305                 goto end;
 306
 307         for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 308                 struct clk_rate_request req_parent;
 309
 310                 clk_hw_forward_rate_request(hw, req, parent, &req_parent, req->rate << shift);
 311                 if (__clk_determine_rate(parent, &req_parent))
 312                         continue;
 313
 314                 clk_sam9x5_peripheral_best_diff(req, parent, req_parent.rate,
 315                                                 shift, &best_diff, &best_rate);
 316
 317                 if (!best_diff)
 318                         break;
 319         }
 320 end:
 321         if (best_rate < 0 ||
 322             (periph->range.max && best_rate > periph->range.max))
 323                 return -EINVAL;
 324
 325         pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
 326                  __func__, best_rate,
 327                  __clk_get_name((req->best_parent_hw)->clk),
 328                  req->best_parent_rate);
 329
 330         req->rate = best_rate;
 331
 332         return 0;
 333 }
 334
 335 static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw,
 336                                              unsigned long rate,
 337                                              unsigned long *parent_rate)
 338 {
 339         int shift = 0;
 340         unsigned long best_rate;
 341         unsigned long best_diff;
 342         unsigned long cur_rate = *parent_rate;
 343         unsigned long cur_diff;
 344         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 345
 346         if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max)
 347                 return *parent_rate;
 348
 349         if (periph->range.max) {
 350                 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 351                         cur_rate = *parent_rate >> shift;
 352                         if (cur_rate <= periph->range.max)
 353                                 break;
 354                 }
 355         }
 356
 357         if (rate >= cur_rate)
 358                 return cur_rate;
 359
 360         best_diff = cur_rate - rate;
 361         best_rate = cur_rate;
 362         for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 363                 cur_rate = *parent_rate >> shift;
 364                 if (cur_rate < rate)
 365                         cur_diff = rate - cur_rate;
 366                 else
 367                         cur_diff = cur_rate - rate;
 368
 369                 if (cur_diff < best_diff) {
 370                         best_diff = cur_diff;
 371                         best_rate = cur_rate;
 372                 }
 373
 374                 if (!best_diff || cur_rate < rate)
 375                         break;
 376         }
 377
 378         return best_rate;
 379 }
 380
 381 static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw,
 382                                           unsigned long rate,
 383                                           unsigned long parent_rate)
 384 {
 385         int shift;
 386         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 387         if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) {
 388                 if (parent_rate == rate)
 389                         return 0;
 390                 else
 391                         return -EINVAL;
 392         }
 393
 394         if (periph->range.max && rate > periph->range.max)
 395                 return -EINVAL;
 396
 397         for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 398                 if (parent_rate >> shift == rate) {
 399                         periph->auto_div = false;
 400                         periph->div = shift;
 401                         return 0;
 402                 }
 403         }
 404
 405         return -EINVAL;
 406 }
 407
 408 static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw)
 409 {
 410         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 411
 412         periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw);
 413
 414         return 0;
 415 }
 416
 417 static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw)
 418 {
 419         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 420
 421         if (periph->pms.status)
 422                 clk_sam9x5_peripheral_set(periph, periph->pms.status);
 423 }
 424
 425 static const struct clk_ops sam9x5_peripheral_ops = {
 426         .enable = clk_sam9x5_peripheral_enable,
 427         .disable = clk_sam9x5_peripheral_disable,
 428         .is_enabled = clk_sam9x5_peripheral_is_enabled,
 429         .recalc_rate = clk_sam9x5_peripheral_recalc_rate,
 430         .round_rate = clk_sam9x5_peripheral_round_rate,
 431         .set_rate = clk_sam9x5_peripheral_set_rate,
 432         .save_context = clk_sam9x5_peripheral_save_context,
 433         .restore_context = clk_sam9x5_peripheral_restore_context,
 434 };
 435
 436 static const struct clk_ops sam9x5_peripheral_chg_ops = {
 437         .enable = clk_sam9x5_peripheral_enable,
 438         .disable = clk_sam9x5_peripheral_disable,
 439         .is_enabled = clk_sam9x5_peripheral_is_enabled,
 440         .recalc_rate = clk_sam9x5_peripheral_recalc_rate,
 441         .determine_rate = clk_sam9x5_peripheral_determine_rate,
 442         .set_rate = clk_sam9x5_peripheral_set_rate,
 443         .save_context = clk_sam9x5_peripheral_save_context,
 444         .restore_context = clk_sam9x5_peripheral_restore_context,
 445 };
 446
 447 struct clk_hw * __init
 448 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
 449                                     const struct clk_pcr_layout *layout,
 450                                     const char *name, const char *parent_name,
 451                                     struct clk_hw *parent_hw,
 452                                     u32 id, const struct clk_range *range,
 453                                     int chg_pid, unsigned long flags)
 454 {
 455         struct clk_sam9x5_peripheral *periph;
 456         struct clk_init_data init = {};
 457         struct clk_hw *hw;
 458         int ret;
 459
 460         if (!name || !(parent_name || parent_hw))
 461                 return ERR_PTR(-EINVAL);
 462
 463         periph = kzalloc(sizeof(*periph), GFP_KERNEL);
 464         if (!periph)
 465                 return ERR_PTR(-ENOMEM);
 466
 467         init.name = name;
 468         if (parent_hw)
 469                 init.parent_hws = (const struct clk_hw **)&parent_hw;
 470         else
 471                 init.parent_names = &parent_name;
 472         init.num_parents = 1;
 473         init.flags = flags;
 474         if (chg_pid < 0) {
 475                 init.ops = &sam9x5_peripheral_ops;
 476         } else {
 477                 init.flags |= CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |
 478                               CLK_SET_RATE_PARENT;
 479                 init.ops = &sam9x5_peripheral_chg_ops;
 480         }
 481
 482         periph->id = id;
 483         periph->hw.init = &init;
 484         periph->div = 0;
 485         periph->regmap = regmap;
 486         periph->lock = lock;
 487         if (layout->div_mask)
 488                 periph->auto_div = true;
 489         periph->layout = layout;
 490         periph->range = *range;
 491         periph->chg_pid = chg_pid;
 492
 493         hw = &periph->hw;
 494         ret = clk_hw_register(NULL, &periph->hw);
 495         if (ret) {
 496                 kfree(periph);
 497                 hw = ERR_PTR(ret);
 498         } else {
 499                 clk_sam9x5_peripheral_autodiv(periph);
 500         }
 501
 502         return hw;
 503 }