drivers/clk/microchip/clk-core.c

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