drivers/clk/microchip/clk-core.c

   1 /*
   2  * Purna Chandra Mandal,<purna.mandal@microchip.com>
   3  * Copyright (C) 2015 Microchip Technology Inc.  All rights reserved.
   4  *
   5  * This program is free software; you can distribute it and/or modify it
   6  * under the terms of the GNU General Public License (Version 2) as
   7  * published by the Free Software Foundation.
   8  *
   9  * This program is distributed in the hope it will be useful, but WITHOUT
  10  * ANY WARRANTY; without even the implied warranty of MERCHANTABILITY or
  11  * FITNESS FOR A PARTICULAR PURPOSE.  See the GNU General Public License
  12  * for more details.
  13  */
  14 #include <linux/clk-provider.h>
  15 #include <linux/delay.h>
  16 #include <linux/device.h>
  17 #include <linux/interrupt.h>
  18 #include <linux/iopoll.h>
  19 #include <asm/mach-pic32/pic32.h>
  20 #include <asm/traps.h>
  21
  22 #include "clk-core.h"
  23
  24 /* OSCCON Reg fields */
  25 #define OSC_CUR_MASK            0x07
  26 #define OSC_CUR_SHIFT           12
  27 #define OSC_NEW_MASK            0x07
  28 #define OSC_NEW_SHIFT           8
  29 #define OSC_SWEN                BIT(0)
  30
  31 /* SPLLCON Reg fields */
  32 #define PLL_RANGE_MASK          0x07
  33 #define PLL_RANGE_SHIFT         0
  34 #define PLL_ICLK_MASK           0x01
  35 #define PLL_ICLK_SHIFT          7
  36 #define PLL_IDIV_MASK           0x07
  37 #define PLL_IDIV_SHIFT          8
  38 #define PLL_ODIV_MASK           0x07
  39 #define PLL_ODIV_SHIFT          24
  40 #define PLL_MULT_MASK           0x7F
  41 #define PLL_MULT_SHIFT          16
  42 #define PLL_MULT_MAX            128
  43 #define PLL_ODIV_MIN            1
  44 #define PLL_ODIV_MAX            5
  45
  46 /* Peripheral Bus Clock Reg Fields */
  47 #define PB_DIV_MASK             0x7f
  48 #define PB_DIV_SHIFT            0
  49 #define PB_DIV_READY            BIT(11)
  50 #define PB_DIV_ENABLE           BIT(15)
  51 #define PB_DIV_MAX              128
  52 #define PB_DIV_MIN              0
  53
  54 /* Reference Oscillator Control Reg fields */
  55 #define REFO_SEL_MASK           0x0f
  56 #define REFO_SEL_SHIFT          0
  57 #define REFO_ACTIVE             BIT(8)
  58 #define REFO_DIVSW_EN           BIT(9)
  59 #define REFO_OE                 BIT(12)
  60 #define REFO_ON                 BIT(15)
  61 #define REFO_DIV_SHIFT          16
  62 #define REFO_DIV_MASK           0x7fff
  63
  64 /* Reference Oscillator Trim Register Fields */
  65 #define REFO_TRIM_REG           0x10
  66 #define REFO_TRIM_MASK          0x1ff
  67 #define REFO_TRIM_SHIFT         23
  68 #define REFO_TRIM_MAX           511
  69
  70 /* Mux Slew Control Register fields */
  71 #define SLEW_BUSY               BIT(0)
  72 #define SLEW_DOWNEN             BIT(1)
  73 #define SLEW_UPEN               BIT(2)
  74 #define SLEW_DIV                0x07
  75 #define SLEW_DIV_SHIFT          8
  76 #define SLEW_SYSDIV             0x0f
  77 #define SLEW_SYSDIV_SHIFT       20
  78
  79 /* Clock Poll Timeout */
  80 #define LOCK_TIMEOUT_US         USEC_PER_MSEC
  81
  82 /* SoC specific clock needed during SPLL clock rate switch */
  83 static struct clk_hw *pic32_sclk_hw;
  84
  85 /* add instruction pipeline delay while CPU clock is in-transition. */
  86 #define cpu_nop5()                      \
  87 do {                                    \
  88         __asm__ __volatile__("nop");    \
  89         __asm__ __volatile__("nop");    \
  90         __asm__ __volatile__("nop");    \
  91         __asm__ __volatile__("nop");    \
  92         __asm__ __volatile__("nop");    \
  93 } while (0)
  94
  95 /* Perpheral bus clocks */
  96 struct pic32_periph_clk {
  97         struct clk_hw hw;
  98         void __iomem *ctrl_reg;
  99         struct pic32_clk_common *core;
 100 };
 101
 102 #define clkhw_to_pbclk(_hw)     container_of(_hw, struct pic32_periph_clk, hw)
 103
 104 static int pbclk_is_enabled(struct clk_hw *hw)
 105 {
 106         struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
 107
 108         return readl(pb->ctrl_reg) & PB_DIV_ENABLE;
 109 }
 110
 111 static int pbclk_enable(struct clk_hw *hw)
 112 {
 113         struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
 114
 115         writel(PB_DIV_ENABLE, PIC32_SET(pb->ctrl_reg));
 116         return 0;
 117 }
 118
 119 static void pbclk_disable(struct clk_hw *hw)
 120 {
 121         struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
 122
 123         writel(PB_DIV_ENABLE, PIC32_CLR(pb->ctrl_reg));
 124 }
 125
 126 static unsigned long calc_best_divided_rate(unsigned long rate,
 127                                             unsigned long parent_rate,
 128                                             u32 divider_max,
 129                                             u32 divider_min)
 130 {
 131         unsigned long divided_rate, divided_rate_down, best_rate;
 132         unsigned long div, div_up;
 133
 134         /* eq. clk_rate = parent_rate / divider.
 135          *
 136          * Find best divider to produce closest of target divided rate.
 137          */
 138         div = parent_rate / rate;
 139         div = clamp_val(div, divider_min, divider_max);
 140         div_up = clamp_val(div + 1, divider_min, divider_max);
 141
 142         divided_rate = parent_rate / div;
 143         divided_rate_down = parent_rate / div_up;
 144         if (abs(rate - divided_rate_down) < abs(rate - divided_rate))
 145                 best_rate = divided_rate_down;
 146         else
 147                 best_rate = divided_rate;
 148
 149         return best_rate;
 150 }
 151
 152 static inline u32 pbclk_read_pbdiv(struct pic32_periph_clk *pb)
 153 {
 154         return ((readl(pb->ctrl_reg) >> PB_DIV_SHIFT) & PB_DIV_MASK) + 1;
 155 }
 156
 157 static unsigned long pbclk_recalc_rate(struct clk_hw *hw,
 158                                        unsigned long parent_rate)
 159 {
 160         struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
 161
 162         return parent_rate / pbclk_read_pbdiv(pb);
 163 }
 164
 165 static long pbclk_round_rate(struct clk_hw *hw, unsigned long rate,
 166                              unsigned long *parent_rate)
 167 {
 168         return calc_best_divided_rate(rate, *parent_rate,
 169                                       PB_DIV_MAX, PB_DIV_MIN);
 170 }
 171
 172 static int pbclk_set_rate(struct clk_hw *hw, unsigned long rate,
 173                           unsigned long parent_rate)
 174 {
 175         struct pic32_periph_clk *pb = clkhw_to_pbclk(hw);
 176         unsigned long flags;
 177         u32 v, div;
 178         int err;
 179
 180         /* check & wait for DIV_READY */
 181         err = readl_poll_timeout(pb->ctrl_reg, v, v & PB_DIV_READY,
 182                                  1, LOCK_TIMEOUT_US);
 183         if (err)
 184                 return err;
 185
 186         /* calculate clkdiv and best rate */
 187         div = DIV_ROUND_CLOSEST(parent_rate, rate);
 188
 189         spin_lock_irqsave(&pb->core->reg_lock, flags);
 190
 191         /* apply new div */
 192         v = readl(pb->ctrl_reg);
 193         v &= ~PB_DIV_MASK;
 194         v |= (div - 1);
 195
 196         pic32_syskey_unlock();
 197
 198         writel(v, pb->ctrl_reg);
 199
 200         spin_unlock_irqrestore(&pb->core->reg_lock, flags);
 201
 202         /* wait again for DIV_READY */
 203         err = readl_poll_timeout(pb->ctrl_reg, v, v & PB_DIV_READY,
 204                                  1, LOCK_TIMEOUT_US);
 205         if (err)
 206                 return err;
 207
 208         /* confirm that new div is applied correctly */
 209         return (pbclk_read_pbdiv(pb) == div) ? 0 : -EBUSY;
 210 }
 211
 212 const struct clk_ops pic32_pbclk_ops = {
 213         .enable         = pbclk_enable,
 214         .disable        = pbclk_disable,
 215         .is_enabled     = pbclk_is_enabled,
 216         .recalc_rate    = pbclk_recalc_rate,
 217         .round_rate     = pbclk_round_rate,
 218         .set_rate       = pbclk_set_rate,
 219 };
 220
 221 struct clk *pic32_periph_clk_register(const struct pic32_periph_clk_data *desc,
 222                                       struct pic32_clk_common *core)
 223 {
 224         struct pic32_periph_clk *pbclk;
 225         struct clk *clk;
 226
 227         pbclk = devm_kzalloc(core->dev, sizeof(*pbclk), GFP_KERNEL);
 228         if (!pbclk)
 229                 return ERR_PTR(-ENOMEM);
 230
 231         pbclk->hw.init = &desc->init_data;
 232         pbclk->core = core;
 233         pbclk->ctrl_reg = desc->ctrl_reg + core->iobase;
 234
 235         clk = devm_clk_register(core->dev, &pbclk->hw);
 236         if (IS_ERR(clk)) {
 237                 dev_err(core->dev, "%s: clk_register() failed\n", __func__);
 238                 devm_kfree(core->dev, pbclk);
 239         }
 240
 241         return clk;
 242 }
 243
 244 /* Reference oscillator operations */
 245 struct pic32_ref_osc {
 246         struct clk_hw hw;
 247         void __iomem *ctrl_reg;
 248         const u32 *parent_map;
 249         struct pic32_clk_common *core;
 250 };
 251
 252 #define clkhw_to_refosc(_hw)    container_of(_hw, struct pic32_ref_osc, hw)
 253
 254 static int roclk_is_enabled(struct clk_hw *hw)
 255 {
 256         struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
 257
 258         return readl(refo->ctrl_reg) & REFO_ON;
 259 }
 260
 261 static int roclk_enable(struct clk_hw *hw)
 262 {
 263         struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
 264
 265         writel(REFO_ON | REFO_OE, PIC32_SET(refo->ctrl_reg));
 266         return 0;
 267 }
 268
 269 static void roclk_disable(struct clk_hw *hw)
 270 {
 271         struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
 272
 273         writel(REFO_ON | REFO_OE, PIC32_CLR(refo->ctrl_reg));
 274 }
 275
 276 static void roclk_init(struct clk_hw *hw)
 277 {
 278         /* initialize clock in disabled state */
 279         roclk_disable(hw);
 280 }
 281
 282 static u8 roclk_get_parent(struct clk_hw *hw)
 283 {
 284         struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
 285         u32 v, i;
 286
 287         v = (readl(refo->ctrl_reg) >> REFO_SEL_SHIFT) & REFO_SEL_MASK;
 288
 289         if (!refo->parent_map)
 290                 return v;
 291
 292         for (i = 0; i < clk_hw_get_num_parents(hw); i++)
 293                 if (refo->parent_map[i] == v)
 294                         return i;
 295
 296         return -EINVAL;
 297 }
 298
 299 static unsigned long roclk_calc_rate(unsigned long parent_rate,
 300                                      u32 rodiv, u32 rotrim)
 301 {
 302         u64 rate64;
 303
 304         /* fout = fin / [2 * {div + (trim / 512)}]
 305          *      = fin * 512 / [1024 * div + 2 * trim]
 306          *      = fin * 256 / (512 * div + trim)
 307          *      = (fin << 8) / ((div << 9) + trim)
 308          */
 309         if (rotrim) {
 310                 rodiv = (rodiv << 9) + rotrim;
 311                 rate64 = parent_rate;
 312                 rate64 <<= 8;
 313                 do_div(rate64, rodiv);
 314         } else if (rodiv) {
 315                 rate64 = parent_rate / (rodiv << 1);
 316         } else {
 317                 rate64 = parent_rate;
 318         }
 319         return rate64;
 320 }
 321
 322 static void roclk_calc_div_trim(unsigned long rate,
 323                                 unsigned long parent_rate,
 324                                 u32 *rodiv_p, u32 *rotrim_p)
 325 {
 326         u32 div, rotrim, rodiv;
 327         u64 frac;
 328
 329         /* Find integer approximation of floating-point arithmetic.
 330          *      fout = fin / [2 * {rodiv + (rotrim / 512)}] ... (1)
 331          * i.e. fout = fin / 2 * DIV
 332          *      whereas DIV = rodiv + (rotrim / 512)
 333          *
 334          * Since kernel does not perform floating-point arithmatic so
 335          * (rotrim/512) will be zero. And DIV & rodiv will result same.
 336          *
 337          * ie. fout = (fin * 256) / [(512 * rodiv) + rotrim]  ... from (1)
 338          * ie. rotrim = ((fin * 256) / fout) - (512 * DIV)
 339          */
 340         if (parent_rate <= rate) {
 341                 div = 0;
 342                 frac = 0;
 343                 rodiv = 0;
 344                 rotrim = 0;
 345         } else {
 346                 div = parent_rate / (rate << 1);
 347                 frac = parent_rate;
 348                 frac <<= 8;
 349                 do_div(frac, rate);
 350                 frac -= (u64)(div << 9);
 351
 352                 rodiv = (div > REFO_DIV_MASK) ? REFO_DIV_MASK : div;
 353                 rotrim = (frac >= REFO_TRIM_MAX) ? REFO_TRIM_MAX : frac;
 354         }
 355
 356         if (rodiv_p)
 357                 *rodiv_p = rodiv;
 358
 359         if (rotrim_p)
 360                 *rotrim_p = rotrim;
 361 }
 362
 363 static unsigned long roclk_recalc_rate(struct clk_hw *hw,
 364                                        unsigned long parent_rate)
 365 {
 366         struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
 367         u32 v, rodiv, rotrim;
 368
 369         /* get rodiv */
 370         v = readl(refo->ctrl_reg);
 371         rodiv = (v >> REFO_DIV_SHIFT) & REFO_DIV_MASK;
 372
 373         /* get trim */
 374         v = readl(refo->ctrl_reg + REFO_TRIM_REG);
 375         rotrim = (v >> REFO_TRIM_SHIFT) & REFO_TRIM_MASK;
 376
 377         return roclk_calc_rate(parent_rate, rodiv, rotrim);
 378 }
 379
 380 static long roclk_round_rate(struct clk_hw *hw, unsigned long rate,
 381                              unsigned long *parent_rate)
 382 {
 383         u32 rotrim, rodiv;
 384
 385         /* calculate dividers for new rate */
 386         roclk_calc_div_trim(rate, *parent_rate, &rodiv, &rotrim);
 387
 388         /* caclulate new rate (rounding) based on new rodiv & rotrim */
 389         return roclk_calc_rate(*parent_rate, rodiv, rotrim);
 390 }
 391
 392 static int roclk_determine_rate(struct clk_hw *hw,
 393                                 struct clk_rate_request *req)
 394 {
 395         struct clk_hw *parent_clk, *best_parent_clk = NULL;
 396         unsigned int i, delta, best_delta = -1;
 397         unsigned long parent_rate, best_parent_rate = 0;
 398         unsigned long best = 0, nearest_rate;
 399
 400         /* find a parent which can generate nearest clkrate >= rate */
 401         for (i = 0; i < clk_hw_get_num_parents(hw); i++) {
 402                 /* get parent */
 403                 parent_clk = clk_hw_get_parent_by_index(hw, i);
 404                 if (!parent_clk)
 405                         continue;
 406
 407                 /* skip if parent runs slower than target rate */
 408                 parent_rate = clk_hw_get_rate(parent_clk);
 409                 if (req->rate > parent_rate)
 410                         continue;
 411
 412                 nearest_rate = roclk_round_rate(hw, req->rate, &parent_rate);
 413                 delta = abs(nearest_rate - req->rate);
 414                 if ((nearest_rate >= req->rate) && (delta < best_delta)) {
 415                         best_parent_clk = parent_clk;
 416                         best_parent_rate = parent_rate;
 417                         best = nearest_rate;
 418                         best_delta = delta;
 419
 420                         if (delta == 0)
 421                                 break;
 422                 }
 423         }
 424
 425         /* if no match found, retain old rate */
 426         if (!best_parent_clk) {
 427                 pr_err("%s:%s, no parent found for rate %lu.\n",
 428                        __func__, clk_hw_get_name(hw), req->rate);
 429                 return clk_hw_get_rate(hw);
 430         }
 431
 432         pr_debug("%s,rate %lu, best_parent(%s, %lu), best %lu, delta %d\n",
 433                  clk_hw_get_name(hw), req->rate,
 434                  clk_hw_get_name(best_parent_clk), best_parent_rate,
 435                  best, best_delta);
 436
 437         if (req->best_parent_rate)
 438                 req->best_parent_rate = best_parent_rate;
 439
 440         if (req->best_parent_hw)
 441                 req->best_parent_hw = best_parent_clk;
 442
 443         return best;
 444 }
 445
 446 static int roclk_set_parent(struct clk_hw *hw, u8 index)
 447 {
 448         struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
 449         unsigned long flags;
 450         u32 v;
 451         int err;
 452
 453         if (refo->parent_map)
 454                 index = refo->parent_map[index];
 455
 456         /* wait until ACTIVE bit is zero or timeout */
 457         err = readl_poll_timeout(refo->ctrl_reg, v, !(v & REFO_ACTIVE),
 458                                  1, LOCK_TIMEOUT_US);
 459         if (err) {
 460                 pr_err("%s: poll failed, clk active\n", clk_hw_get_name(hw));
 461                 return err;
 462         }
 463
 464         spin_lock_irqsave(&refo->core->reg_lock, flags);
 465
 466         pic32_syskey_unlock();
 467
 468         /* calculate & apply new */
 469         v = readl(refo->ctrl_reg);
 470         v &= ~(REFO_SEL_MASK << REFO_SEL_SHIFT);
 471         v |= index << REFO_SEL_SHIFT;
 472
 473         writel(v, refo->ctrl_reg);
 474
 475         spin_unlock_irqrestore(&refo->core->reg_lock, flags);
 476
 477         return 0;
 478 }
 479
 480 static int roclk_set_rate_and_parent(struct clk_hw *hw,
 481                                      unsigned long rate,
 482                                      unsigned long parent_rate,
 483                                      u8 index)
 484 {
 485         struct pic32_ref_osc *refo = clkhw_to_refosc(hw);
 486         unsigned long flags;
 487         u32 trim, rodiv, v;
 488         int err;
 489
 490         /* calculate new rodiv & rotrim for new rate */
 491         roclk_calc_div_trim(rate, parent_rate, &rodiv, &trim);
 492
 493         pr_debug("parent_rate = %lu, rate = %lu, div = %d, trim = %d\n",
 494                  parent_rate, rate, rodiv, trim);
 495
 496         /* wait till source change is active */
 497         err = readl_poll_timeout(refo->ctrl_reg, v,
 498                                  !(v & (REFO_ACTIVE | REFO_DIVSW_EN)),
 499                                  1, LOCK_TIMEOUT_US);
 500         if (err) {
 501                 pr_err("%s: poll timedout, clock is still active\n", __func__);
 502                 return err;
 503         }
 504
 505         spin_lock_irqsave(&refo->core->reg_lock, flags);
 506         v = readl(refo->ctrl_reg);
 507
 508         pic32_syskey_unlock();
 509
 510         /* apply parent, if required */
 511         if (refo->parent_map)
 512                 index = refo->parent_map[index];
 513
 514         v &= ~(REFO_SEL_MASK << REFO_SEL_SHIFT);
 515         v |= index << REFO_SEL_SHIFT;
 516
 517         /* apply RODIV */
 518         v &= ~(REFO_DIV_MASK << REFO_DIV_SHIFT);
 519         v |= rodiv << REFO_DIV_SHIFT;
 520         writel(v, refo->ctrl_reg);
 521
 522         /* apply ROTRIM */
 523         v = readl(refo->ctrl_reg + REFO_TRIM_REG);
 524         v &= ~(REFO_TRIM_MASK << REFO_TRIM_SHIFT);
 525         v |= trim << REFO_TRIM_SHIFT;
 526         writel(v, refo->ctrl_reg + REFO_TRIM_REG);
 527
 528         /* enable & activate divider switching */
 529         writel(REFO_ON | REFO_DIVSW_EN, PIC32_SET(refo->ctrl_reg));
 530
 531         /* wait till divswen is in-progress */
 532         err = readl_poll_timeout_atomic(refo->ctrl_reg, v, !(v & REFO_DIVSW_EN),
 533                                         1, LOCK_TIMEOUT_US);
 534         /* leave the clk gated as it was */
 535         writel(REFO_ON, PIC32_CLR(refo->ctrl_reg));
 536
 537         spin_unlock_irqrestore(&refo->core->reg_lock, flags);
 538
 539         return err;
 540 }
 541
 542 static int roclk_set_rate(struct clk_hw *hw, unsigned long rate,
 543                           unsigned long parent_rate)
 544 {
 545         u8 index = roclk_get_parent(hw);
 546
 547         return roclk_set_rate_and_parent(hw, rate, parent_rate, index);
 548 }
 549
 550 const struct clk_ops pic32_roclk_ops = {
 551         .enable                 = roclk_enable,
 552         .disable                = roclk_disable,
 553         .is_enabled             = roclk_is_enabled,
 554         .get_parent             = roclk_get_parent,
 555         .set_parent             = roclk_set_parent,
 556         .determine_rate         = roclk_determine_rate,
 557         .recalc_rate            = roclk_recalc_rate,
 558         .set_rate_and_parent    = roclk_set_rate_and_parent,
 559         .set_rate               = roclk_set_rate,
 560         .init                   = roclk_init,
 561 };
 562
 563 struct clk *pic32_refo_clk_register(const struct pic32_ref_osc_data *data,
 564                                     struct pic32_clk_common *core)
 565 {
 566         struct pic32_ref_osc *refo;
 567         struct clk *clk;
 568
 569         refo = devm_kzalloc(core->dev, sizeof(*refo), GFP_KERNEL);
 570         if (!refo)
 571                 return ERR_PTR(-ENOMEM);
 572
 573         refo->core = core;
 574         refo->hw.init = &data->init_data;
 575         refo->ctrl_reg = data->ctrl_reg + core->iobase;
 576         refo->parent_map = data->parent_map;
 577
 578         clk = devm_clk_register(core->dev, &refo->hw);
 579         if (IS_ERR(clk))
 580                 dev_err(core->dev, "%s: clk_register() failed\n", __func__);
 581
 582         return clk;
 583 }
 584
 585 struct pic32_sys_pll {
 586         struct clk_hw hw;
 587         void __iomem *ctrl_reg;
 588         void __iomem *status_reg;
 589         u32 lock_mask;
 590         u32 idiv; /* PLL iclk divider, treated fixed */
 591         struct pic32_clk_common *core;
 592 };
 593
 594 #define clkhw_to_spll(_hw)      container_of(_hw, struct pic32_sys_pll, hw)
 595
 596 static inline u32 spll_odiv_to_divider(u32 odiv)
 597 {
 598         odiv = clamp_val(odiv, PLL_ODIV_MIN, PLL_ODIV_MAX);
 599
 600         return 1 << odiv;
 601 }
 602
 603 static unsigned long spll_calc_mult_div(struct pic32_sys_pll *pll,
 604                                         unsigned long rate,
 605                                         unsigned long parent_rate,
 606                                         u32 *mult_p, u32 *odiv_p)
 607 {
 608         u32 mul, div, best_mul = 1, best_div = 1;
 609         unsigned long new_rate, best_rate = rate;
 610         unsigned int best_delta = -1, delta, match_found = 0;
 611         u64 rate64;
 612
 613         parent_rate /= pll->idiv;
 614
 615         for (mul = 1; mul <= PLL_MULT_MAX; mul++) {
 616                 for (div = PLL_ODIV_MIN; div <= PLL_ODIV_MAX; div++) {
 617                         rate64 = parent_rate;
 618                         rate64 *= mul;
 619                         do_div(rate64, 1 << div);
 620                         new_rate = rate64;
 621                         delta = abs(rate - new_rate);
 622                         if ((new_rate >= rate) && (delta < best_delta)) {
 623                                 best_delta = delta;
 624                                 best_rate = new_rate;
 625                                 best_mul = mul;
 626                                 best_div = div;
 627                                 match_found = 1;
 628                         }
 629                 }
 630         }
 631
 632         if (!match_found) {
 633                 pr_warn("spll: no match found\n");
 634                 return 0;
 635         }
 636
 637         pr_debug("rate %lu, par_rate %lu/mult %u, div %u, best_rate %lu\n",
 638                  rate, parent_rate, best_mul, best_div, best_rate);
 639
 640         if (mult_p)
 641                 *mult_p = best_mul - 1;
 642
 643         if (odiv_p)
 644                 *odiv_p = best_div;
 645
 646         return best_rate;
 647 }
 648
 649 static unsigned long spll_clk_recalc_rate(struct clk_hw *hw,
 650                                           unsigned long parent_rate)
 651 {
 652         struct pic32_sys_pll *pll = clkhw_to_spll(hw);
 653         unsigned long pll_in_rate;
 654         u32 mult, odiv, div, v;
 655         u64 rate64;
 656
 657         v = readl(pll->ctrl_reg);
 658         odiv = ((v >> PLL_ODIV_SHIFT) & PLL_ODIV_MASK);
 659         mult = ((v >> PLL_MULT_SHIFT) & PLL_MULT_MASK) + 1;
 660         div = spll_odiv_to_divider(odiv);
 661
 662         /* pll_in_rate = parent_rate / idiv
 663          * pll_out_rate = pll_in_rate * mult / div;
 664          */
 665         pll_in_rate = parent_rate / pll->idiv;
 666         rate64 = pll_in_rate;
 667         rate64 *= mult;
 668         do_div(rate64, div);
 669
 670         return rate64;
 671 }
 672
 673 static long spll_clk_round_rate(struct clk_hw *hw, unsigned long rate,
 674                                 unsigned long *parent_rate)
 675 {
 676         struct pic32_sys_pll *pll = clkhw_to_spll(hw);
 677
 678         return spll_calc_mult_div(pll, rate, *parent_rate, NULL, NULL);
 679 }
 680
 681 static int spll_clk_set_rate(struct clk_hw *hw, unsigned long rate,
 682                              unsigned long parent_rate)
 683 {
 684         struct pic32_sys_pll *pll = clkhw_to_spll(hw);
 685         unsigned long ret, flags;
 686         u32 mult, odiv, v;
 687         int err;
 688
 689         ret = spll_calc_mult_div(pll, rate, parent_rate, &mult, &odiv);
 690         if (!ret)
 691                 return -EINVAL;
 692
 693         /*
 694          * We can't change SPLL counters when it is in-active use
 695          * by SYSCLK. So check before applying new counters/rate.
 696          */
 697
 698         /* Is spll_clk active parent of sys_clk ? */
 699         if (unlikely(clk_hw_get_parent(pic32_sclk_hw) == hw)) {
 700                 pr_err("%s: failed, clk in-use\n", __func__);
 701                 return -EBUSY;
 702         }
 703
 704         spin_lock_irqsave(&pll->core->reg_lock, flags);
 705
 706         /* apply new multiplier & divisor */
 707         v = readl(pll->ctrl_reg);
 708         v &= ~(PLL_MULT_MASK << PLL_MULT_SHIFT);
 709         v &= ~(PLL_ODIV_MASK << PLL_ODIV_SHIFT);
 710         v |= (mult << PLL_MULT_SHIFT) | (odiv << PLL_ODIV_SHIFT);
 711
 712         /* sys unlock before write */
 713         pic32_syskey_unlock();
 714
 715         writel(v, pll->ctrl_reg);
 716         cpu_relax();
 717
 718         /* insert few nops (5-stage) to ensure CPU does not hang */
 719         cpu_nop5();
 720         cpu_nop5();
 721
 722         /* Wait until PLL is locked (maximum 100 usecs). */
 723         err = readl_poll_timeout_atomic(pll->status_reg, v,
 724                                         v & pll->lock_mask, 1, 100);
 725         spin_unlock_irqrestore(&pll->core->reg_lock, flags);
 726
 727         return err;
 728 }
 729
 730 /* SPLL clock operation */
 731 const struct clk_ops pic32_spll_ops = {
 732         .recalc_rate    = spll_clk_recalc_rate,
 733         .round_rate     = spll_clk_round_rate,
 734         .set_rate       = spll_clk_set_rate,
 735 };
 736
 737 struct clk *pic32_spll_clk_register(const struct pic32_sys_pll_data *data,
 738                                     struct pic32_clk_common *core)
 739 {
 740         struct pic32_sys_pll *spll;
 741         struct clk *clk;
 742
 743         spll = devm_kzalloc(core->dev, sizeof(*spll), GFP_KERNEL);
 744         if (!spll)
 745                 return ERR_PTR(-ENOMEM);
 746
 747         spll->core = core;
 748         spll->hw.init = &data->init_data;
 749         spll->ctrl_reg = data->ctrl_reg + core->iobase;
 750         spll->status_reg = data->status_reg + core->iobase;
 751         spll->lock_mask = data->lock_mask;
 752
 753         /* cache PLL idiv; PLL driver uses it as constant.*/
 754         spll->idiv = (readl(spll->ctrl_reg) >> PLL_IDIV_SHIFT) & PLL_IDIV_MASK;
 755         spll->idiv += 1;
 756
 757         clk = devm_clk_register(core->dev, &spll->hw);
 758         if (IS_ERR(clk))
 759                 dev_err(core->dev, "sys_pll: clk_register() failed\n");
 760
 761         return clk;
 762 }
 763
 764 /* System mux clock(aka SCLK) */
 765
 766 struct pic32_sys_clk {
 767         struct clk_hw hw;
 768         void __iomem *mux_reg;
 769         void __iomem *slew_reg;
 770         u32 slew_div;
 771         const u32 *parent_map;
 772         struct pic32_clk_common *core;
 773 };
 774
 775 #define clkhw_to_sys_clk(_hw)   container_of(_hw, struct pic32_sys_clk, hw)
 776
 777 static unsigned long sclk_get_rate(struct clk_hw *hw, unsigned long parent_rate)
 778 {
 779         struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
 780         u32 div;
 781
 782         div = (readl(sclk->slew_reg) >> SLEW_SYSDIV_SHIFT) & SLEW_SYSDIV;
 783         div += 1; /* sys-div to divider */
 784
 785         return parent_rate / div;
 786 }
 787
 788 static long sclk_round_rate(struct clk_hw *hw, unsigned long rate,
 789                             unsigned long *parent_rate)
 790 {
 791         return calc_best_divided_rate(rate, *parent_rate, SLEW_SYSDIV, 1);
 792 }
 793
 794 static int sclk_set_rate(struct clk_hw *hw,
 795                          unsigned long rate, unsigned long parent_rate)
 796 {
 797         struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
 798         unsigned long flags;
 799         u32 v, div;
 800         int err;
 801
 802         div = parent_rate / rate;
 803
 804         spin_lock_irqsave(&sclk->core->reg_lock, flags);
 805
 806         /* apply new div */
 807         v = readl(sclk->slew_reg);
 808         v &= ~(SLEW_SYSDIV << SLEW_SYSDIV_SHIFT);
 809         v |= (div - 1) << SLEW_SYSDIV_SHIFT;
 810
 811         pic32_syskey_unlock();
 812
 813         writel(v, sclk->slew_reg);
 814
 815         /* wait until BUSY is cleared */
 816         err = readl_poll_timeout_atomic(sclk->slew_reg, v,
 817                                         !(v & SLEW_BUSY), 1, LOCK_TIMEOUT_US);
 818
 819         spin_unlock_irqrestore(&sclk->core->reg_lock, flags);
 820
 821         return err;
 822 }
 823
 824 static u8 sclk_get_parent(struct clk_hw *hw)
 825 {
 826         struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
 827         u32 i, v;
 828
 829         v = (readl(sclk->mux_reg) >> OSC_CUR_SHIFT) & OSC_CUR_MASK;
 830
 831         if (!sclk->parent_map)
 832                 return v;
 833
 834         for (i = 0; i < clk_hw_get_num_parents(hw); i++)
 835                 if (sclk->parent_map[i] == v)
 836                         return i;
 837         return -EINVAL;
 838 }
 839
 840 static int sclk_set_parent(struct clk_hw *hw, u8 index)
 841 {
 842         struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
 843         unsigned long flags;
 844         u32 nosc, cosc, v;
 845         int err;
 846
 847         spin_lock_irqsave(&sclk->core->reg_lock, flags);
 848
 849         /* find new_osc */
 850         nosc = sclk->parent_map ? sclk->parent_map[index] : index;
 851
 852         /* set new parent */
 853         v = readl(sclk->mux_reg);
 854         v &= ~(OSC_NEW_MASK << OSC_NEW_SHIFT);
 855         v |= nosc << OSC_NEW_SHIFT;
 856
 857         pic32_syskey_unlock();
 858
 859         writel(v, sclk->mux_reg);
 860
 861         /* initate switch */
 862         writel(OSC_SWEN, PIC32_SET(sclk->mux_reg));
 863         cpu_relax();
 864
 865         /* add nop to flush pipeline (as cpu_clk is in-flux) */
 866         cpu_nop5();
 867
 868         /* wait for SWEN bit to clear */
 869         err = readl_poll_timeout_atomic(sclk->slew_reg, v,
 870                                         !(v & OSC_SWEN), 1, LOCK_TIMEOUT_US);
 871
 872         spin_unlock_irqrestore(&sclk->core->reg_lock, flags);
 873
 874         /*
 875          * SCLK clock-switching logic might reject a clock switching request
 876          * if pre-requisites (like new clk_src not present or unstable) are
 877          * not met.
 878          * So confirm before claiming success.
 879          */
 880         cosc = (readl(sclk->mux_reg) >> OSC_CUR_SHIFT) & OSC_CUR_MASK;
 881         if (cosc != nosc) {
 882                 pr_err("%s: err, failed to set_parent() to %d, current %d\n",
 883                        clk_hw_get_name(hw), nosc, cosc);
 884                 err = -EBUSY;
 885         }
 886
 887         return err;
 888 }
 889
 890 static void sclk_init(struct clk_hw *hw)
 891 {
 892         struct pic32_sys_clk *sclk = clkhw_to_sys_clk(hw);
 893         unsigned long flags;
 894         u32 v;
 895
 896         /* Maintain reference to this clk, required in spll_clk_set_rate() */
 897         pic32_sclk_hw = hw;
 898
 899         /* apply slew divider on both up and down scaling */
 900         if (sclk->slew_div) {
 901                 spin_lock_irqsave(&sclk->core->reg_lock, flags);
 902                 v = readl(sclk->slew_reg);
 903                 v &= ~(SLEW_DIV << SLEW_DIV_SHIFT);
 904                 v |= sclk->slew_div << SLEW_DIV_SHIFT;
 905                 v |= SLEW_DOWNEN | SLEW_UPEN;
 906                 writel(v, sclk->slew_reg);
 907                 spin_unlock_irqrestore(&sclk->core->reg_lock, flags);
 908         }
 909 }
 910
 911 /* sclk with post-divider */
 912 const struct clk_ops pic32_sclk_ops = {
 913         .get_parent     = sclk_get_parent,
 914         .set_parent     = sclk_set_parent,
 915         .round_rate     = sclk_round_rate,
 916         .set_rate       = sclk_set_rate,
 917         .recalc_rate    = sclk_get_rate,
 918         .init           = sclk_init,
 919         .determine_rate = __clk_mux_determine_rate,
 920 };
 921
 922 /* sclk with no slew and no post-divider */
 923 const struct clk_ops pic32_sclk_no_div_ops = {
 924         .get_parent     = sclk_get_parent,
 925         .set_parent     = sclk_set_parent,
 926         .init           = sclk_init,
 927         .determine_rate = __clk_mux_determine_rate,
 928 };
 929
 930 struct clk *pic32_sys_clk_register(const struct pic32_sys_clk_data *data,
 931                                    struct pic32_clk_common *core)
 932 {
 933         struct pic32_sys_clk *sclk;
 934         struct clk *clk;
 935
 936         sclk = devm_kzalloc(core->dev, sizeof(*sclk), GFP_KERNEL);
 937         if (!sclk)
 938                 return ERR_PTR(-ENOMEM);
 939
 940         sclk->core = core;
 941         sclk->hw.init = &data->init_data;
 942         sclk->mux_reg = data->mux_reg + core->iobase;
 943         sclk->slew_reg = data->slew_reg + core->iobase;
 944         sclk->slew_div = data->slew_div;
 945         sclk->parent_map = data->parent_map;
 946
 947         clk = devm_clk_register(core->dev, &sclk->hw);
 948         if (IS_ERR(clk))
 949                 dev_err(core->dev, "%s: clk register failed\n", __func__);
 950
 951         return clk;
 952 }
 953
 954 /* secondary oscillator */
 955 struct pic32_sec_osc {
 956         struct clk_hw hw;
 957         void __iomem *enable_reg;
 958         void __iomem *status_reg;
 959         u32 enable_mask;
 960         u32 status_mask;
 961         unsigned long fixed_rate;
 962         struct pic32_clk_common *core;
 963 };
 964
 965 #define clkhw_to_sosc(_hw)      container_of(_hw, struct pic32_sec_osc, hw)
 966 static int sosc_clk_enable(struct clk_hw *hw)
 967 {
 968         struct pic32_sec_osc *sosc = clkhw_to_sosc(hw);
 969         u32 v;
 970
 971         /* enable SOSC */
 972         pic32_syskey_unlock();
 973         writel(sosc->enable_mask, PIC32_SET(sosc->enable_reg));
 974
 975         /* wait till warm-up period expires or ready-status is updated */
 976         return readl_poll_timeout_atomic(sosc->status_reg, v,
 977                                          v & sosc->status_mask, 1, 100);
 978 }
 979
 980 static void sosc_clk_disable(struct clk_hw *hw)
 981 {
 982         struct pic32_sec_osc *sosc = clkhw_to_sosc(hw);
 983
 984         pic32_syskey_unlock();
 985         writel(sosc->enable_mask, PIC32_CLR(sosc->enable_reg));
 986 }
 987
 988 static int sosc_clk_is_enabled(struct clk_hw *hw)
 989 {
 990         struct pic32_sec_osc *sosc = clkhw_to_sosc(hw);
 991         u32 enabled, ready;
 992
 993         /* check enabled and ready status */
 994         enabled = readl(sosc->enable_reg) & sosc->enable_mask;
 995         ready = readl(sosc->status_reg) & sosc->status_mask;
 996
 997         return enabled && ready;
 998 }
 999
1000 static unsigned long sosc_clk_calc_rate(struct clk_hw *hw,
1001                                         unsigned long parent_rate)
1002 {
1003         return clkhw_to_sosc(hw)->fixed_rate;
1004 }
1005
1006 const struct clk_ops pic32_sosc_ops = {
1007         .enable = sosc_clk_enable,
1008         .disable = sosc_clk_disable,
1009         .is_enabled = sosc_clk_is_enabled,
1010         .recalc_rate = sosc_clk_calc_rate,
1011 };
1012
1013 struct clk *pic32_sosc_clk_register(const struct pic32_sec_osc_data *data,
1014                                     struct pic32_clk_common *core)
1015 {
1016         struct pic32_sec_osc *sosc;
1017
1018         sosc = devm_kzalloc(core->dev, sizeof(*sosc), GFP_KERNEL);
1019         if (!sosc)
1020                 return ERR_PTR(-ENOMEM);
1021
1022         sosc->core = core;
1023         sosc->hw.init = &data->init_data;
1024         sosc->fixed_rate = data->fixed_rate;
1025         sosc->enable_mask = data->enable_mask;
1026         sosc->status_mask = data->status_mask;
1027         sosc->enable_reg = data->enable_reg + core->iobase;
1028         sosc->status_reg = data->status_reg + core->iobase;
1029
1030         return devm_clk_register(core->dev, &sosc->hw);
1031 }