drivers/clk/at91/clk-peripheral.c

   1 // SPDX-License-Identifier: GPL-2.0-or-later
   2 /*
   3  *  Copyright (C) 2013 Boris BREZILLON <[email protected]>
   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, u32 id)
 101 {
 102         struct clk_peripheral *periph;
 103         struct clk_init_data init;
 104         struct clk_hw *hw;
 105         int ret;
 106
 107         if (!name || !parent_name || id > PERIPHERAL_ID_MAX)
 108                 return ERR_PTR(-EINVAL);
 109
 110         periph = kzalloc(sizeof(*periph), GFP_KERNEL);
 111         if (!periph)
 112                 return ERR_PTR(-ENOMEM);
 113
 114         init.name = name;
 115         init.ops = &peripheral_ops;
 116         init.parent_names = &parent_name;
 117         init.num_parents = 1;
 118         init.flags = 0;
 119
 120         periph->id = id;
 121         periph->hw.init = &init;
 122         periph->regmap = regmap;
 123
 124         hw = &periph->hw;
 125         ret = clk_hw_register(NULL, &periph->hw);
 126         if (ret) {
 127                 kfree(periph);
 128                 hw = ERR_PTR(ret);
 129         }
 130
 131         return hw;
 132 }
 133
 134 static void clk_sam9x5_peripheral_autodiv(struct clk_sam9x5_peripheral *periph)
 135 {
 136         struct clk_hw *parent;
 137         unsigned long parent_rate;
 138         int shift = 0;
 139
 140         if (!periph->auto_div)
 141                 return;
 142
 143         if (periph->range.max) {
 144                 parent = clk_hw_get_parent_by_index(&periph->hw, 0);
 145                 parent_rate = clk_hw_get_rate(parent);
 146                 if (!parent_rate)
 147                         return;
 148
 149                 for (; shift < PERIPHERAL_MAX_SHIFT; shift++) {
 150                         if (parent_rate >> shift <= periph->range.max)
 151                                 break;
 152                 }
 153         }
 154
 155         periph->auto_div = false;
 156         periph->div = shift;
 157 }
 158
 159 static int clk_sam9x5_peripheral_set(struct clk_sam9x5_peripheral *periph,
 160                                      unsigned int status)
 161 {
 162         unsigned long flags;
 163         unsigned int enable = status ? AT91_PMC_PCR_EN : 0;
 164
 165         if (periph->id < PERIPHERAL_ID_MIN)
 166                 return 0;
 167
 168         spin_lock_irqsave(periph->lock, flags);
 169         regmap_write(periph->regmap, periph->layout->offset,
 170                      (periph->id & periph->layout->pid_mask));
 171         regmap_update_bits(periph->regmap, periph->layout->offset,
 172                            periph->layout->div_mask | periph->layout->cmd |
 173                            enable,
 174                            field_prep(periph->layout->div_mask, periph->div) |
 175                            periph->layout->cmd | enable);
 176         spin_unlock_irqrestore(periph->lock, flags);
 177
 178         return 0;
 179 }
 180
 181 static int clk_sam9x5_peripheral_enable(struct clk_hw *hw)
 182 {
 183         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 184
 185         return clk_sam9x5_peripheral_set(periph, 1);
 186 }
 187
 188 static void clk_sam9x5_peripheral_disable(struct clk_hw *hw)
 189 {
 190         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 191         unsigned long flags;
 192
 193         if (periph->id < PERIPHERAL_ID_MIN)
 194                 return;
 195
 196         spin_lock_irqsave(periph->lock, flags);
 197         regmap_write(periph->regmap, periph->layout->offset,
 198                      (periph->id & periph->layout->pid_mask));
 199         regmap_update_bits(periph->regmap, periph->layout->offset,
 200                            AT91_PMC_PCR_EN | periph->layout->cmd,
 201                            periph->layout->cmd);
 202         spin_unlock_irqrestore(periph->lock, flags);
 203 }
 204
 205 static int clk_sam9x5_peripheral_is_enabled(struct clk_hw *hw)
 206 {
 207         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 208         unsigned long flags;
 209         unsigned int status;
 210
 211         if (periph->id < PERIPHERAL_ID_MIN)
 212                 return 1;
 213
 214         spin_lock_irqsave(periph->lock, flags);
 215         regmap_write(periph->regmap, periph->layout->offset,
 216                      (periph->id & periph->layout->pid_mask));
 217         regmap_read(periph->regmap, periph->layout->offset, &status);
 218         spin_unlock_irqrestore(periph->lock, flags);
 219
 220         return !!(status & AT91_PMC_PCR_EN);
 221 }
 222
 223 static unsigned long
 224 clk_sam9x5_peripheral_recalc_rate(struct clk_hw *hw,
 225                                   unsigned long parent_rate)
 226 {
 227         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 228         unsigned long flags;
 229         unsigned int status;
 230
 231         if (periph->id < PERIPHERAL_ID_MIN)
 232                 return parent_rate;
 233
 234         spin_lock_irqsave(periph->lock, flags);
 235         regmap_write(periph->regmap, periph->layout->offset,
 236                      (periph->id & periph->layout->pid_mask));
 237         regmap_read(periph->regmap, periph->layout->offset, &status);
 238         spin_unlock_irqrestore(periph->lock, flags);
 239
 240         if (status & AT91_PMC_PCR_EN) {
 241                 periph->div = field_get(periph->layout->div_mask, status);
 242                 periph->auto_div = false;
 243         } else {
 244                 clk_sam9x5_peripheral_autodiv(periph);
 245         }
 246
 247         return parent_rate >> periph->div;
 248 }
 249
 250 static void clk_sam9x5_peripheral_best_diff(struct clk_rate_request *req,
 251                                             struct clk_hw *parent,
 252                                             unsigned long parent_rate,
 253                                             u32 shift, long *best_diff,
 254                                             long *best_rate)
 255 {
 256         unsigned long tmp_rate = parent_rate >> shift;
 257         unsigned long tmp_diff = abs(req->rate - tmp_rate);
 258
 259         if (*best_diff < 0 || *best_diff >= tmp_diff) {
 260                 *best_rate = tmp_rate;
 261                 *best_diff = tmp_diff;
 262                 req->best_parent_rate = parent_rate;
 263                 req->best_parent_hw = parent;
 264         }
 265 }
 266
 267 static int clk_sam9x5_peripheral_determine_rate(struct clk_hw *hw,
 268                                                 struct clk_rate_request *req)
 269 {
 270         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 271         struct clk_hw *parent = clk_hw_get_parent(hw);
 272         struct clk_rate_request req_parent = *req;
 273         unsigned long parent_rate = clk_hw_get_rate(parent);
 274         unsigned long tmp_rate;
 275         long best_rate = LONG_MIN;
 276         long best_diff = LONG_MIN;
 277         u32 shift;
 278
 279         if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max)
 280                 return parent_rate;
 281
 282         /* Fist step: check the available dividers. */
 283         for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 284                 tmp_rate = parent_rate >> shift;
 285
 286                 if (periph->range.max && tmp_rate > periph->range.max)
 287                         continue;
 288
 289                 clk_sam9x5_peripheral_best_diff(req, parent, parent_rate,
 290                                                 shift, &best_diff, &best_rate);
 291
 292                 if (!best_diff || best_rate <= req->rate)
 293                         break;
 294         }
 295
 296         if (periph->chg_pid < 0)
 297                 goto end;
 298
 299         /* Step two: try to request rate from parent. */
 300         parent = clk_hw_get_parent_by_index(hw, periph->chg_pid);
 301         if (!parent)
 302                 goto end;
 303
 304         for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 305                 req_parent.rate = req->rate << shift;
 306
 307                 if (__clk_determine_rate(parent, &req_parent))
 308                         continue;
 309
 310                 clk_sam9x5_peripheral_best_diff(req, parent, req_parent.rate,
 311                                                 shift, &best_diff, &best_rate);
 312
 313                 if (!best_diff)
 314                         break;
 315         }
 316 end:
 317         if (best_rate < 0 ||
 318             (periph->range.max && best_rate > periph->range.max))
 319                 return -EINVAL;
 320
 321         pr_debug("PCK: %s, best_rate = %ld, parent clk: %s @ %ld\n",
 322                  __func__, best_rate,
 323                  __clk_get_name((req->best_parent_hw)->clk),
 324                  req->best_parent_rate);
 325
 326         req->rate = best_rate;
 327
 328         return 0;
 329 }
 330
 331 static long clk_sam9x5_peripheral_round_rate(struct clk_hw *hw,
 332                                              unsigned long rate,
 333                                              unsigned long *parent_rate)
 334 {
 335         int shift = 0;
 336         unsigned long best_rate;
 337         unsigned long best_diff;
 338         unsigned long cur_rate = *parent_rate;
 339         unsigned long cur_diff;
 340         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 341
 342         if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max)
 343                 return *parent_rate;
 344
 345         if (periph->range.max) {
 346                 for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 347                         cur_rate = *parent_rate >> shift;
 348                         if (cur_rate <= periph->range.max)
 349                                 break;
 350                 }
 351         }
 352
 353         if (rate >= cur_rate)
 354                 return cur_rate;
 355
 356         best_diff = cur_rate - rate;
 357         best_rate = cur_rate;
 358         for (; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 359                 cur_rate = *parent_rate >> shift;
 360                 if (cur_rate < rate)
 361                         cur_diff = rate - cur_rate;
 362                 else
 363                         cur_diff = cur_rate - rate;
 364
 365                 if (cur_diff < best_diff) {
 366                         best_diff = cur_diff;
 367                         best_rate = cur_rate;
 368                 }
 369
 370                 if (!best_diff || cur_rate < rate)
 371                         break;
 372         }
 373
 374         return best_rate;
 375 }
 376
 377 static int clk_sam9x5_peripheral_set_rate(struct clk_hw *hw,
 378                                           unsigned long rate,
 379                                           unsigned long parent_rate)
 380 {
 381         int shift;
 382         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 383         if (periph->id < PERIPHERAL_ID_MIN || !periph->range.max) {
 384                 if (parent_rate == rate)
 385                         return 0;
 386                 else
 387                         return -EINVAL;
 388         }
 389
 390         if (periph->range.max && rate > periph->range.max)
 391                 return -EINVAL;
 392
 393         for (shift = 0; shift <= PERIPHERAL_MAX_SHIFT; shift++) {
 394                 if (parent_rate >> shift == rate) {
 395                         periph->auto_div = false;
 396                         periph->div = shift;
 397                         return 0;
 398                 }
 399         }
 400
 401         return -EINVAL;
 402 }
 403
 404 static int clk_sam9x5_peripheral_save_context(struct clk_hw *hw)
 405 {
 406         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 407
 408         periph->pms.status = clk_sam9x5_peripheral_is_enabled(hw);
 409
 410         return 0;
 411 }
 412
 413 static void clk_sam9x5_peripheral_restore_context(struct clk_hw *hw)
 414 {
 415         struct clk_sam9x5_peripheral *periph = to_clk_sam9x5_peripheral(hw);
 416
 417         if (periph->pms.status)
 418                 clk_sam9x5_peripheral_set(periph, periph->pms.status);
 419 }
 420
 421 static const struct clk_ops sam9x5_peripheral_ops = {
 422         .enable = clk_sam9x5_peripheral_enable,
 423         .disable = clk_sam9x5_peripheral_disable,
 424         .is_enabled = clk_sam9x5_peripheral_is_enabled,
 425         .recalc_rate = clk_sam9x5_peripheral_recalc_rate,
 426         .round_rate = clk_sam9x5_peripheral_round_rate,
 427         .set_rate = clk_sam9x5_peripheral_set_rate,
 428         .save_context = clk_sam9x5_peripheral_save_context,
 429         .restore_context = clk_sam9x5_peripheral_restore_context,
 430 };
 431
 432 static const struct clk_ops sam9x5_peripheral_chg_ops = {
 433         .enable = clk_sam9x5_peripheral_enable,
 434         .disable = clk_sam9x5_peripheral_disable,
 435         .is_enabled = clk_sam9x5_peripheral_is_enabled,
 436         .recalc_rate = clk_sam9x5_peripheral_recalc_rate,
 437         .determine_rate = clk_sam9x5_peripheral_determine_rate,
 438         .set_rate = clk_sam9x5_peripheral_set_rate,
 439         .save_context = clk_sam9x5_peripheral_save_context,
 440         .restore_context = clk_sam9x5_peripheral_restore_context,
 441 };
 442
 443 struct clk_hw * __init
 444 at91_clk_register_sam9x5_peripheral(struct regmap *regmap, spinlock_t *lock,
 445                                     const struct clk_pcr_layout *layout,
 446                                     const char *name, const char *parent_name,
 447                                     u32 id, const struct clk_range *range,
 448                                     int chg_pid)
 449 {
 450         struct clk_sam9x5_peripheral *periph;
 451         struct clk_init_data init;
 452         struct clk_hw *hw;
 453         int ret;
 454
 455         if (!name || !parent_name)
 456                 return ERR_PTR(-EINVAL);
 457
 458         periph = kzalloc(sizeof(*periph), GFP_KERNEL);
 459         if (!periph)
 460                 return ERR_PTR(-ENOMEM);
 461
 462         init.name = name;
 463         init.parent_names = &parent_name;
 464         init.num_parents = 1;
 465         if (chg_pid < 0) {
 466                 init.flags = 0;
 467                 init.ops = &sam9x5_peripheral_ops;
 468         } else {
 469                 init.flags = CLK_SET_RATE_GATE | CLK_SET_PARENT_GATE |
 470                              CLK_SET_RATE_PARENT;
 471                 init.ops = &sam9x5_peripheral_chg_ops;
 472         }
 473
 474         periph->id = id;
 475         periph->hw.init = &init;
 476         periph->div = 0;
 477         periph->regmap = regmap;
 478         periph->lock = lock;
 479         if (layout->div_mask)
 480                 periph->auto_div = true;
 481         periph->layout = layout;
 482         periph->range = *range;
 483         periph->chg_pid = chg_pid;
 484
 485         hw = &periph->hw;
 486         ret = clk_hw_register(NULL, &periph->hw);
 487         if (ret) {
 488                 kfree(periph);
 489                 hw = ERR_PTR(ret);
 490         } else {
 491                 clk_sam9x5_peripheral_autodiv(periph);
 492         }
 493
 494         return hw;
 495 }